目次
前
次
ArduinoMega2560サーボモータ処理
ArduinoMega2560を使ってサーボモータを動かしました。
利用したサーボモータは、MiniS RB303
手元にあったので、ArduinoMega2560からの５Ｖで
動作することを確認しました。
ArduinoMega2560の接続ピンは、11ピンとしました。
14、15ピンには、スイッチをつけ、押すと指定した
角度10度だけ増減するようにします。
スイッチの接続回路は、以下。
負論理で動作するようにしてみました。
入力と出力を決めたので、関数pinModeで入出力
方向を指定します。
#define POUT 11
#define RBIN 14
#define LBIN 15
/* direction */
pinMode(POUT,OUTPUT);
pinMode(RBIN,INPUT_PULLUP);
pinMode(LBIN,INPUT_PULLUP);
/* value */
digitalWrite(POUT,LOW);
サーボモータを利用するので、用意されている
ライブラリを使い、クラスとして認識させます。
#include <Servo.h>
Servo xSrv ;
２個のスイッチで、サーボモータの角度を増減
させるだけでなく、端末から角度を指定できる
ようにもしたいので、シリアル通信ライブラリ
を利用します。
関数setupの中は、次のように定義。
void setup()
{
Serial.begin(9600);
rs_puts("Hello !");
crlf();
show_help();
/* direction */
pinMode(POUT,OUTPUT);
pinMode(RBIN,INPUT_PULLUP);
pinMode(LBIN,INPUT_PULLUP);
/* value */
digitalWrite(POUT,LOW);
/* flags */
uflag = OFF ;
tflag = OFF ;
/* */
xcnt = 0 ;
lsft = 0 ;
rsft = 0 ;
/* initialize servo motor angle */
xSrv.attach(POUT,1000,2000);
xangle = 90 ;
xSrv.write(xangle);
/* 10ms period */
MsTimer2::set(10,update_trigger);
MsTimer2::start();
}
サーボモータは、１ｍｓから２ｍｓのパルス幅を
与えると、０度から１８０度まで回転するとし
クラスメソッドで、ピン番号と動作範囲を指定
しています。
ボタンの状態値が変化したことを捉えて、回転
角度を増減したいので、シフトレジスタでの
変化をキャッチします。シフトレジスタ用変数
をlsft、rsftとしています。
シフトレジスタで、信号の変化をキャッチする
には、クロックが必要。タイマー割込みでの
キャッチができるように、MsTimer2を活用して
みます。
割込み周期を１０ｍｓにして、基板にあるLEDを
点滅させることを考えました。
MsTimer2から呼び出される関数は、以下。
void update_trigger()
{
/* set flag */
tflag = ON ;
/* inrement */
xcnt++ ;
/* flashing */
digitalWrite(POUT,LOW) ;
if ( xcnt > 0x80 ) { digitalWrite(POUT,HIGH) ; }
}
１０ｍｓごとに、関数loopの中にある
ブロックに、フラグtflagで通知します。
イベント通知フラグで呼び出されたときには
次のようにシフトレジスタのLSBに、スイッチ
の状態を保存。
if ( tflag == ON ) {
/* clear */
tflag = OFF ;
/* shift */
lsft <<= 1 ;
rsft <<= 1 ;
/* mask */
lsft &= 3 ;
rsft &= 3 ;
/* get both switch */
tmp = digitalRead(LBIN) ;
tmp <<= 1 ;
tmp |= digitalRead(RBIN) ;
/* judge */
if ( bitRead(tmp,1) == 0 ) { lsft++ ; }
if ( bitRead(tmp,0) == 0 ) { rsft++ ; }
/* update servo motor angle */
if ( lsft == 1 && rsft == 0 ) { xangle += 10 ; }
if ( lsft == 0 && rsft == 1 ) { xangle -= 10 ; }
/* range check */
if ( xangle > 180 ) { xangle = 180 ; }
if ( xangle < 0 ) { xangle = 0 ; }
/* update */
xSrv.write(xangle);
}
シフトレジスタは、２つあるので、シフトとマスクを
実行後、LSBに各々のスイッチの状態を格納します。
押されたボタンに応じて、角度を増減してからレンジ
をチェックし、サーボモータの角度処理メソッドへと
角度を転送。
端末ソフトでも、サーボモータを動かしたいので
１文字コマンドを、次のように定義。
- ? コマンドリスト表示
- A ０から１８０を１文字コマンドの後に入力し角度とする
- a その時点でのサーボモータの角度を表示
コマンドを定義したなら、コマンドインタプリタを作成。
if ( uflag == ON ) {
/* clear flag */
uflag = OFF ;
/* get command */
cmd = *(sbuf+0) ;
/* help */
if ( cmd == '?' ) { show_help(); }
/* set angle */
if ( cmd == 'A' ) { set_angle(); }
/* show angle */
if ( cmd == 'a' ) { get_angle(); }
/* new line */
crlf() ;
}
コマンド処理を担当する関数は、次のように定義。
void show_help()
{
rs_puts("? help") ; crlf();
rs_puts("A set angle") ; crlf();
rs_puts("a get angle") ; crlf();
}
void set_angle()
{
byte aa;
byte result ;
byte ii ;
/* default */
result = 0 ;
/* calulate */
for ( ii = 0 ; ii < 3 ; ii++ ) {
/* get ascii code */
aa = *(sbuf+1+ii) ;
/* judge */
if ( aa == '\r' ) break ;
/* calculate */
result = result * 10 + get_hex( aa ) ;
}
/* range check */
if ( result > 180 ) { result = 180 ; }
/* set */
xangle = result ;
xSrv.write( xangle ) ;
}
void get_angle()
{
byte aa[4];
byte tmp ;
byte ii ;
/* default */
*(aa+3) = '\0' ;
/* get current angle */
tmp = xSrv.read();
/* separate */
*(aa+2) = tmp % 10 ; tmp /= 10 ;
*(aa+1) = tmp % 10 ; tmp /= 10 ;
*(aa+0) = tmp % 10 ;
/* convert */
for ( ii = 0 ; ii < 3 ; ii++ ) {
*(aa+ii) = get_asc( *(aa+ii) );
}
/* zero surpress */
if ( *(aa+0) == '0' ) {
*(aa+0) = ' ' ;
if ( *(aa+1) == '0' ) { *(aa+1) = ' ' ; }
}
/* show */
rs_puts( aa );
/* new line */
crlf();
}
コマンドに対応する関数内では、サーボモータ用クラス
を利用して、角度指定か、角度取得かを設定するだけ。
複雑にすると、バグが入りやすくなるので、短いコード
で対応しています。
必要な関数を追加して、次のようにまとめられます。
#include <MsTimer2.h>
#include <Servo.h>
#define OFF 0
#define ON OFF+1
#define LED_BIT 5
#define POUT 11
#define RBIN 14
#define LBIN 15
Servo xSrv ;
/* variables */
boolean uflag ;
boolean tflag ;
byte xcnt ;
char sbuf[8] ;
byte sindex ;
byte cmd ;
int xangle ;
byte lsft ;
byte rsft ;
void update_trigger();
void show_help();
void rs_putchar(char x);
void rs_puts(char *ptr);
void crlf();
void show_val();
void rs_print(byte x);
byte get_hex(char x);
char get_asc(byte x);
void set_angle();
void get_angle();
void setup()
{
Serial.begin(9600);
rs_puts("Hello !");
crlf();
show_help();
/* direction */
pinMode(POUT,OUTPUT);
pinMode(RBIN,INPUT_PULLUP);
pinMode(LBIN,INPUT_PULLUP);
/* value */
digitalWrite(POUT,LOW);
/* flags */
uflag = OFF ;
tflag = OFF ;
/* */
xcnt = 0 ;
lsft = 0 ;
rsft = 0 ;
/* initialize servo motor angle */
xSrv.attach(POUT,1000,2000);
xangle = 90 ;
xSrv.write(xangle);
/* 10ms period */
MsTimer2::set(10,update_trigger);
MsTimer2::start();
}
void loop()
{
byte tmp ;
/* command interpreter */
if ( uflag == ON ) {
/* clear flag */
uflag = OFF ;
/* get command */
cmd = *(sbuf+0) ;
/* help */
if ( cmd == '?' ) { show_help(); }
/* set angle */
if ( cmd == 'A' ) { set_angle(); }
/* show angle */
if ( cmd == 'a' ) { get_angle(); }
/* new line */
crlf() ;
}
/* timer handling */
if ( tflag == ON ) {
/* clear */
tflag = OFF ;
/* shift */
lsft <<= 1 ;
rsft <<= 1 ;
/* mask */
lsft &= 3 ;
rsft &= 3 ;
/* get both switch */
tmp = digitalRead(LBIN) ;
tmp <<= 1 ;
tmp |= digitalRead(RBIN) ;
/* judge */
if ( bitRead(tmp,1) == 0 ) { lsft++ ; }
if ( bitRead(tmp,0) == 0 ) { rsft++ ; }
/* update servo motor angle */
if ( lsft == 1 && rsft == 0 ) { xangle += 10 ; }
if ( lsft == 0 && rsft == 1 ) { xangle -= 10 ; }
/* range check */
if ( xangle > 180 ) { xangle = 180 ; }
if ( xangle < 0 ) { xangle = 0 ; }
/* update */
xSrv.write(xangle);
}
}
void update_trigger()
{
/* set flag */
tflag = ON ;
/* inrement */
xcnt++ ;
/* flashing */
digitalWrite(POUT,LOW) ;
if ( xcnt > 0x80 ) { digitalWrite(POUT,HIGH) ; }
}
void show_help()
{
rs_puts("? help") ; crlf();
rs_puts("A set angle") ; crlf();
rs_puts("a get angle") ; crlf();
}
void rs_putchar(char x)
{
Serial.write(x);
}
void rs_puts(char *ptr)
{
while ( *ptr ) {
rs_putchar( *ptr );
ptr++;
}
}
void crlf()
{
rs_putchar('\r');
rs_putchar('\n');
}
byte get_hex(char x)
{
byte result ;
/* default */
result = 0 ;
/* judge */
if ( '0' <= x && x <= '9' ) { result = x - '0' ; }
if ( 'A' <= x && x <= 'F' ) { result = x - 'A' + 10 ; }
if ( 'a' <= x && x <= 'f' ) { result = x - 'a' + 10 ; }
return result ;
}
char get_asc(byte x)
{
char result ;
/* default */
result = '0' ;
/* judge */
if ( x < 10 ) { result = x + '0' ; }
else {
result = x - 10 + 'A' ;
}
return result ;
}
void set_angle()
{
byte aa;
byte result ;
byte ii ;
/* default */
result = 0 ;
/* calulate */
for ( ii = 0 ; ii < 3 ; ii++ ) {
/* get ascii code */
aa = *(sbuf+1+ii) ;
/* judge */
if ( aa == '\r' ) break ;
/* calculate */
result = result * 10 + get_hex( aa ) ;
}
/* range check */
if ( result > 180 ) { result = 180 ; }
/* set */
xangle = result ;
xSrv.write( xangle ) ;
}
void get_angle()
{
byte aa[4];
byte tmp ;
byte ii ;
/* default */
*(aa+3) = '\0' ;
/* get current angle */
tmp = xSrv.read();
/* separate */
*(aa+2) = tmp % 10 ; tmp /= 10 ;
*(aa+1) = tmp % 10 ; tmp /= 10 ;
*(aa+0) = tmp % 10 ;
/* convert */
for ( ii = 0 ; ii < 3 ; ii++ ) {
*(aa+ii) = get_asc( *(aa+ii) );
}
/* zero surpress */
if ( *(aa+0) == '0' ) {
*(aa+0) = ' ' ;
if ( *(aa+1) == '0' ) { *(aa+1) = ' ' ; }
}
/* show */
rs_puts( aa );
/* new line */
crlf();
}
/* receive interrupt */
void serialEvent()
{
char ch;
if ( Serial.available() > 0 ) {
/* get 1 character */
ch = Serial.read();
/* store */
*(sbuf+sindex) = ch ;
/* increment */
sindex++ ;
/* judge */
if ( ch == '\r' ) {
sindex = 0 ;
uflag = ON ;
}
}
}
目次
前
次