STM32F4 DISCOVERYでラジコンサーボを動かす

ロボット作ろう：シェーキー製作記

今回はPWM機能を使ってラジコンサーボを動かしてみます。

これはSTM32F4-Discovery_FW_V1.1.0のProject→Peripheral_ExamplesのTIM_PWM_Outpuｔというサンプルプログラムを調整・整理しただけです。TIMER3のPWMチャネルを４つ使い、４台のサーボを直接駆動できるようになっています。

各サーボのパルス幅は下記のようにservopos1-4というグローバル変数を定義しておき、初期値として4500を入れておきます。これでパルス幅が約1500usになります。

uint16_t servopos1 = 4500;
uint16_t servopos2 = 4500;
uint16_t servopos3 = 4500;
uint16_t servopos4 = 4500;

まず、GPIOポートの初期化です。これはTIM3のPWM出力を有効にするものですが、１－２チャネルと３－４チャネルが別のポートに割り当てになっているため、それぞれ同じような処理をするので長いコードになっています。

void TIM_Port_Config(void)
{
  GPIO_InitTypeDef GPIO_InitStructure;

  //TIM3にクロック供給開始
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);

  //GPIOCとGPIOBにクロック供給開始
  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC | RCC_AHB1Periph_GPIOB, ENABLE);

  //GPIOCのPin6とPin7をオルタネィテブファンクション(OUT)に設定
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 ;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;
  GPIO_Init(GPIOC, &GPIO_InitStructure);

  //GPIOBのPin0とPin1をオルタネィテブファンクション(OUT)に設定
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;
  GPIO_Init(GPIOB, &GPIO_InitStructure);

  //設定したピンをTIM3の出力として設定
  GPIO_PinAFConfig(GPIOC, GPIO_PinSource6, GPIO_AF_TIM3);
  GPIO_PinAFConfig(GPIOC, GPIO_PinSource7, GPIO_AF_TIM3);
  GPIO_PinAFConfig(GPIOB, GPIO_PinSource0, GPIO_AF_TIM3);
  GPIO_PinAFConfig(GPIOB, GPIO_PinSource1, GPIO_AF_TIM3);
}

次が肝心カナメのTIM3の設定です。この設定は繰返し周期20ms、PWMの分解能0.3333usに設定します。したがって、サーボのパルス幅を　800us（左最大）-1500us（中立）-2200us（右最大）としたとき、PWMの設定値は2400-4500-6600となります。角度やパーセンテージなどお望みの単位系からこの値に変換する関数を作れば、簡単に使えますね。設定は次のようになりました。

void TIM_Param_Config(){
//タイムベースの設定
TIM_TimeBaseStructure.TIM_Period = 0xe8ff;
TIM_TimeBaseStructure.TIM_Prescaler = 8;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);

//PWM1 Modeの設定・チャネル１
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = servopos1;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OC1Init(TIM3, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);

//PWM1 Modeの設定・チャネル２
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = servopos2;
TIM_OC2Init(TIM3, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);

//PWM1 Modeの設定・チャネル３
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = servopos3;
TIM_OC3Init(TIM3, &TIM_OCInitStructure);
TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);

//PWM1 Modeの設定・チャネル４
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = servopos4;
TIM_OC4Init(TIM3, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);

TIM_ARRPreloadConfig(TIM3, ENABLE);

//TIM3を有効化
TIM_Cmd(TIM3, ENABLE);
}

実はこの設定、マニュアルを読まずにオシロで波形を見ながらトライアルアンドエラーで値を決めました。いただきものの吟醸酒をお供に、３０分ほどで設定値を決めることができました。快適です。やっぱりわれわれアマチュアにオシロは必需品だなあと再認識した次第です。

テストはチャネル１(PC6)に接続したサーボを、約１秒おきにニュートラルと約45度の位置へ交互に動かして行いました。そのmain関数がこれです。ここで呼んでいるmyLongDelayは、以前の時間待ち関数の定数を4000000にして１秒ほど待たせるものです。

int main(void)
{
  //タイマー用I/Oの設定
  TIM_Port_Config();
  //タイマーのパラメータを設定
  TIM_Param_Config();
  //チャネル１のサーボを動かす
  while (1)
  {
   //チャネル１(PC6)のサーボをニュートラル(1500us)から333us動かす
   //分解能は0.3333us。↓では0.3333 * 1000 = 333.3us
   servopos1 += 1000;
   TIM_OCInitStructure.TIM_Pulse = servopos1;
   TIM_OC1Init(TIM3, &TIM_OCInitStructure);

   myLongDelay(); //約１秒待つ

   //ニュートラルに戻す
   servopos1 -= 1000;
   TIM_OCInitStructure.TIM_Pulse = servopos1;
   TIM_OC1Init(TIM3, &TIM_OCInitStructure);

   myLongDelay(); //約１秒待つ
  }
}

ほかのチャネルも同様に値を設定すればサーボを動かすことができます。今回は４つまでです。ほかのタイマーを同様に設定すればもっと数を増やせそうですが、ヒューマノイドロボットを作るわけではないので、今回は検討しません。シェーキーでは頭のパン・チルトだけなのでサーボは二つしか使いません。

今回はこの写真のように、電池ボックスの乾電池から6Vを供給したラジコンサーボの信号に、 3.3V系のSTM32F4を直接つないで動かしました。サーボの信号スレショルドが何ボルトかわかりませんが、GWSのサーボはこれで動くようです。サーボによっては5V近くの電圧が必要になるかもしれません。その場合は5V系の電圧を設け、ポート設定をオープンドレインにして10Kくらいで5Vへプルアップすればよいでしょう。

GPIO、シリアル、タイマー割り込み、サーボ、と試しましたので、次はADですね。これをテストすればロボットの制御に使えそうです。

【後日追記】

初期化構造体などをグローバルにとっているので、この説明だけではわかりにくいですね。下のmain.c（他のファイルと重複しないようファイル名はservo_main.cになっていますが、プロジェクトのmain.cファイルです）を見てもらえば、もっとわかりやすいと思います。

「servo_main.c」をダウンロード