MSP430で4つのRCサーボモータを制御する

このソフトは…

MSP430G2231を使って、いわゆるRCサーボを駆動するための指令パルスを発生させる ものです。RCサーボでちょっとしたおもちゃを作りたいときなどにどうぞ。 14pinのDIPの石一つで4つのモータをまとめて相手にできます。
以下、仕様。

すごいところ

MSP430G2231はハードウェアとしては独立したPWMを1つしか発生できません。 このソフトはタイマ割り込みを使った力技で無理やり4ch作っています。 入力のステップが64と粗いのは そのせい(スピードの問題)です。実際は少しだけマージン多めに取っているので、 128 段階もいけるかもしれません(interruptADC()の最初のところの0xfff0を0xfff8に すると128段階になります)。

ソース

// A/D & PWM 
#include <io.h>
#include <signal.h>

#define kAD0         BIT0
#define kAD1         BIT1
#define kAD2         BIT2
#define kAD3         BIT3
#define kPWM0        BIT4
#define kPWM1        BIT5
#define kPWM2        BIT6
#define kPWM3        BIT7
#define kPWMPeriod   9531    // 15.25 / 8 / 2 = 953,125 Hz
#define kPWMMin       953    // about 1 ms

unsigned int gAD[4];
unsigned int gPWM[4];
unsigned char gPWMBit[4];

void initializeMCU(void)
{
  // setup clock
  WDTCTL = WDTPW + WDTHOLD;     // shut up the dog
  BCSCTL1 = RSEL3 + RSEL2 + RSEL1 + RSEL0;  // RSEL=15 for 15.25 MHz
  BCSCTL2 = DIVS_3;             // SMCLC = DCO / 8
  BCSCTL3 |= LFXT1S_2;          // Select VLO for ACLK
  // setup I/O
  P1DIR = kPWM0 + kPWM1 + kPWM2 + kPWM3;
  P2SEL &= ~(BIT6 + BIT7);
  // setup analog input
  ADC10AE0  |= kAD0 + kAD1 + kAD2 + kAD3;
  ADC10CTL1  = INCH_3 + CONSEQ_1;  // ADC10CLK
  // setup TIMER_A
  TACTL = TASSEL__SMCLK + ID_1 + MC__CONTINOUS; // 15.25 / 8 / 2 = 953,125 Hz
  TACCR0 = (unsigned int)kPWMPeriod;
  TACCTL0 |= CCIE;
}

interrupt(TIMERA0_VECTOR) interruptTimerA0(void)
{
  static char cnt = 3;
  int i;
  if(cnt == 4){
    TACTL |= TACLR;   // reset TimerA
    cnt = 0;
    TACCR0 = gPWM[0];
    P1OUT |= kPWM0 + kPWM1 + kPWM2 + kPWM3;
  }else{
    // search next CCR0 value
    for(i = 0, cnt = 0; i < 4; i++){
      if(TACCR0 < gPWM[i]){
	TACCR0 = gPWM[i];
	break;
      }
      P1OUT &= gPWMBit[i];
      cnt++;
      if(cnt == 4){
	TACCR0 = (unsigned int)kPWMPeriod;
	ADC10CTL0 |= ADC10SHT_DIV64 + ADC10ON + ADC10IE + ENC + MSC + ADC10SC;  // start A/D
      }
    }
  }
}

interrupt(ADC10_VECTOR) interruptADC()
{
  static int ch = 4;
  unsigned int i, j, tmp;
  unsigned char tmpc;

  gAD[--ch] = ADC10MEM & 0xfff0;
  if(ch == 0){
    ch = 4;
    gPWMBit[0] = ~kPWM0;
    gPWMBit[1] = ~kPWM1;
    gPWMBit[2] = ~kPWM2;
    gPWMBit[3] = (unsigned char)~kPWM3;
    // sort
    for(i = 0; i < 4; i++){
      for(j = i + 1; j < 4; j++){
	if(gAD[i] > gAD[j]){
	  tmp = gAD[j];
	  gAD[j] = gAD[i];
	  gAD[i] = tmp;
	  tmpc = gPWMBit[j];
	  gPWMBit[j] = gPWMBit[i];
	  gPWMBit[i] = tmpc;
	}
      }
      gPWM[i] = gAD[i] + kPWMMin;
    }
  }
}

int main(void)
{
  initializeMCU();
  _BIS_SR(GIE);
  for(;;) _BIS_SR(SCG0 | CPUOFF); // goes in LPM1
  return 0;
}

手抜きの動作解説

下図の▲のところで割り込みがかかります。以上。
fig: How-it-works-diagram of 4ch RC servo controller


『マイコンを使った開発について』に戻る

Valid XHTML 1.0!