夏休みに工業系先生向けの制御講習会を行うけど、 例年参加している人もでてくるので、少しは違う実験とすべく、 サーボモータの制御を入れてみようと計画中。 ということで、手持ちである共立電子の通信販売のサーボモータ(プチロボ)を動かしてみた。

#include <Servo.h>
Servo motor ;    // White D9
int   d = 1 ;    // Red   5V
int   v = 0 ;    // Black GND
void setup() {
motor.attach( 9 ) ;
}
void loop() { // 0°〜180°〜0°を繰り返す。
motor.write( v ) ;
v = v + d ;
if ( v >= 180 ) {
v = 179 ;
d = -1 ;
} else if ( v < 0 ) {
v = 0 ;
d = 1 ;
}
delay( 2 ) ;
}

これで動かすと、このサーボモータでは、プログラム的には0-180で変化させているが、実際の変化幅は120°程であった。 delay(1)にすると、サーボの制御信号の変化にモータが追いつけないため、 90°ほどの変化幅となった。

ちなみに、最近購入したArduino Uno であったが、開発環境は Arduino 0022 に 更新しないと、Uno への書き込みができなかった。(0017はダメ)

加速度センサーと連動

加速度センサーで傾きを検出し、傾きに応じてサーボモータを動かしてみた。

#include <Servo.h>
Servo motor ;
int acc_x ;  // Vcc--3.3V, GS1-GND g1.5
int acc_y ;  // GND--GND , GS2-GND
int acc_z ;  // X----AN0 , SLP-3.3V
int ang ;    // Y----AN1
// Z----AN2
#define OFFSET 335
void setup() {
motor.attach( 9 ) ;
acc_x = acc_y = acc_z = 0 ;
}
void loop() {
acc_x = (3*acc_x + analogRead(0) - OFFSET )/4 ; // 加重移動平均
acc_y = (3*acc_y + analogRead(1) - OFFSET )/4 ;
acc_z = (3*acc_z + analogRead(2) - OFFSET )/4 ;
ang = (int)( atan2( acc_y , acc_x ) / 3.141592 * 180 ) ;
ang = constrain( ang , 0 , 180 ) ;
delay( 2 ) ;
}

リスト操作の演習中だけど、残り授業もあと(今日を入れて)あと２回ということで、 リスト関連のネタが未消化にならないように、スタックとキューについて説明。

スタック

配列を用いた、LIFO(Last In First Out)=スタックであれば、一般的に 以下のようなコードになる。

int stack[ 100 ] ;
int sp = 0 ;
void push( int x ) {
stack[ sp++ ] = x ;
}
int pop() {
return stack[ --sp ] ;
}

しかし、この方法では、配列サイズ以上のデータは保存できない。 これをリストを使うことでサイズを気にしないスタックを実現できる。

struct List* stack = NULL ;
void push( int x ) {
stack = cons( x , stack ) ;
}
int pop() {
int ans = stack->data ;
struct List* del = stack ;
stack = stack->next ;
free( del ) ;
return ans ;
}

キュー

待ち行列(Queue)は、FIFO(First In First Out)を配列で実装する場合、 一般的には、以下のようになる。ただしエラー対策は記載していないので、要注意。

int que[ 100 ] ;
int wp = 0 ; // 書き込み用ポインタ
int rp = 0 ; // 読み出し用ポインタ
void put( int x ) {
que[ wp ] = x ;
wp = (wp + 1) % 100 ; // 循環させる
}
int get() {
int ans = que[ rp ] ;
rp = (rp + 1) % 100 ; // 循環させる
return ans ;
}

このような配列の領域を使い切ったら、先頭から再利用するような方法は、 リングバッファなどと呼ばれる。 このような待ち行列は、キー入力バッファや、プロセス待ち行列などに よく利用される。しかし、このプログラムでも、配列サイズ以上の データは保存できないので、リストを用いる。

struct List* top = NULL ;
struct List** tail = &top ;
void put( int x ) {
*tail = cons( x , NULL ) ;
tail = &( (*tail)->next ) ;
}
int get() {
int ans = top->data ;
struct List* del = top ;
top = top->next ;
free( del ) ;
return ans ;
}

ただし、このプログラムは、常に１件以上データがリストに入っている場合は 問題がないが、get() を実行して、データ件数が０件になると、tail の指す先が おかしくなるので注意が必要。

また、待ち行列では、先頭ポインタと末尾ポインタの２つが必要であるが、 リスト構造の末尾のNULLを、先頭データを指すようにする循環リストと する場合も多い。特に、プロセス待ち行列を実装するときのラウンドロビン方式 などでは、末尾まで処理が及んだ次は先頭に戻って処理を行うため、 循環リストは都合がいい。