MSP430で4つのRCサーボモータ制御
このソフトは……
MSP430G2231を使って、いわゆるRCサーボを駆動するための指令パルスを発生させる ものです。RCサーボでちょっとしたおもちゃを作りたいときなどにどうぞ。 14pinのDIPの石一つで4つのモータをまとめて相手にできます。
以下、仕様。
- 4chのアナログ入力(P1.0〜P1.3, 0〜Vcc)に応じて、
- 4chのRCサーボPWM(P1.4〜P1.7, だいたい1〜2ms幅、10ms周期)を発生します。
- 入力の粒度は6bit(64段階)です。
すごいところ
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;
}
手抜きの動作解説
下図の▲のところで割り込みがかかります。以上。
