2018年10月
  1 2 3 4 5 6
7 8 9 10 11 12 13
14 15 16 17 18 19 20
21 22 23 24 25 26 27
28 29 30 31      

カテゴリー

ブログパーツ

無料ブログはココログ

« STM32F4 DISCOVERYのシリアル通信を割り込みで実装する | トップページ | STM32F4 DISCOVERYのAD変換をテストする »

2011年12月23日 (金)

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

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

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

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

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

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

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

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の設定・チャネル1
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の設定・チャネル2
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の設定・チャネル3
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の設定・チャネル4
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);
}

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

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

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

   myLongDelay(); //約1秒待つ

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

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

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

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

Dscn1981

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

【後日追記】

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

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

« STM32F4 DISCOVERYのシリアル通信を割り込みで実装する | トップページ | STM32F4 DISCOVERYのAD変換をテストする »

ロボット作ろう」カテゴリの記事

コメント

コメントを書く

(ウェブ上には掲載しません)

トラックバック

この記事のトラックバックURL:
http://app.f.cocolog-nifty.com/t/trackback/1140145/43421175

この記事へのトラックバック一覧です: STM32F4 DISCOVERYでラジコンサーボを動かす:

« STM32F4 DISCOVERYのシリアル通信を割り込みで実装する | トップページ | STM32F4 DISCOVERYのAD変換をテストする »