RC サーボの制御

tech | 02:05 |

いくつかサーボモータを購入していたのだけど、ようやく試した。持っているのは上記 SG90 と GWS03T/2BBMG/JR。どちらも動かしてみた。

これらはどちらも JR というタイプ (ピンアサインの種類)。種類はこのページの線のタイプついてを見た。

典型的なプロトコルとしては 20ms ごとに 1000μs〜2000μs (1500μs が中立) のパルスを送るというものらしい (可動角度60度)。

とりあえず試そうと、16bit PWM タイマーを使ってやってみた。

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#define clear_bit(v, bit) v &= ~(1 << bit)
#define set_bit(v, bit)   v |=  (1 << bit)

#define INPUT_NOR PB0
#define INPUT_REV PB1

static inline void setup_io () {
	/**
	 * Data Direction Register: 0=input, 1=output
	 * 必要なポートだけインプットポートにする。
	 */
	DDRB  = 0b11111100;
	DDRC  = 0b11111111;
	DDRD  = 0b11111111;

	PORTB = 0b00000011;
	PORTC = 0b00000000;
	PORTD = 0b00000000;

	uint16_t TIMER1_MODE = 14;
	TCCR1A = (0<<COM1A1) | (0<<COM1A0) | (1<<COM1B1) | (0<<COM1B0) | (((TIMER1_MODE>>1)&1)<<WGM11) | ((TIMER1_MODE&1)<<WGM10);
	TCCR1B = (((TIMER1_MODE>>3)&1)<<WGM13) | (((TIMER1_MODE>>2)&1)<<WGM12) | (0<<CS12) | (1<<CS11) | (0<<CS10);
	ICR1   = 20000; // Top
	OCR1B  = 1500;
	TIMSK1 = (0<<OCIE1B) | (0<<OCIE1A) | (0<<TOIE1);

	sei();
}

static inline void set_position(uint16_t angle) {
	int base = 600;
	int max  = 2400;
	uint16_t n = ((uint32_t)angle * max) / 180 + base;
	if (n < base) {
		n = base;
	} else
	if (n > max) {
		n = max;
	}
	OCR1B = n;
}

int main(void) {
	setup_io();
	set_position(0);

	for (;;) {
		if (bit_is_clear(PINB, INPUT_NOR)) {
			set_position(0);
		}

		if (bit_is_clear(PINB, INPUT_REV)) {

			set_position(180);
		}

		_delay_ms(10);
	}
}

ハマったポイント

可動角度

ググると 1000μs〜2000μs のパルスというサイトばかりヒットするが、これは可動角度60度の場合で、実際は 600us〜2400μs までのパルスを受け付け、180度可動可能なものが多いみたい？よくわからないけど、手元の2種類についてはだいたいこの範囲のパルスを受けつけた。範囲外のパルスを入力すると「ジジジジ」とか「ブブブブ」とかいって止まり、消費電力が多くなる。

電圧

電圧が足りないと、指定した角度付近まで移動したあと「ジジジジ」と止まってしまうことがあった。

制御方法

16bit PWM だと1つの AVR で2つまでしか駆動できないので、割込みを使った別の方法を検討してもよさそう。

Arduino だと特に何も考えなくてもたくさんサーボを制御できるみたい。