目次
前
次
マニュピュレータロボット制御スケッチ
サーボモータを多数利用した身近な例は
マニュピュレータロボットでしょう。
手元にあるマニュピュレータロボットは
サーボモータを5個利用した5軸です。
サーボモータを制御するには、関数analogWriteを
利用しますが、PWM波形を出力できるピンは固定
されています。
5、6、9、10、11の5ピンを
サーボモータ制御に割り当てます。
Arduinoの出力では、サーボモータを駆動できる
パワーがないので、フォトカプラ利用ドライブ
回路を接続します。
回路は簡単で、抵抗とフォトカプラがあるだけ。
フォトカプラを入れるのは、Arduinoを大電力回路
からのバックラッシュから保護するため。
Arduinoとは、14ピンケーブルとブレッドボード
ワイヤーを使い接続します。
マニュピュレータで利用しているサーボモータは
12V電源なので、ACアダプターを駆動回路に
接続します。
サーボモータからは、3ピンワイヤーが出ています。
中央に電源、左右にGNDとパルス入力が配置されています。
左右を間違えてコネクタに接続しても、モータが
電源の逆挿しで破損しないための、工夫です。
Arduinoで5個のサーボモータを制御するので
ピンアサインを決めます。
5 Finger
6 Wrist
9 Arm
10 Elbow
11 Body
ピンアサイン仕様から、Arduinoのコードで使える
記述を定義します。
#define Finger 5
#define Wrist 6
#define Arm 9
#define Elbow 10
#define Body 11
5個のサーボモータは、角度で0〜180まで扱えるので
対応する変数を用意しておきます。
int angleFinger ;
int angleWrist ;
int angleArm ;
int angleElbow ;
int angleBody ;
コマンドインタプリタで、サーボモータの各軸の
角度を指定された場合の処理を考えます。
コマンドインタプリタで利用する1文字コマンド
を次のように決めました。
- ? help
- f set finger degree
- w set wrist degree
- a set arm degree
- e set elbow degree
- b set body degree
コマンドインタプリタで参照する文字列が
配列sbufに含まれているとし、処理を定義
します。
// get command
cmd = sbuf[0] ;
// judge
if ( cmd == '?' ) { show_help() ; }
if ( is_scmd(cmd) == ON ) {
val = get_value() ;
if ( val > MAXANGLE ) { val = MAXANGLE ; }
switch ( cmd ) {
case 'f' : angleFinger = val ; break ;
case 'w' : angleWrist = val ; break ;
case 'a' : angleArm = val ; break ;
case 'e' : angleElbow = val ; break ;
case 'b' : angleBody = val ; break ;
}
}
コマンドインタプリタで利用している関数を
定義していきます。
show_help
コマンドの説明を表示すればよいので
Serialオブジェクトのprintlnメソッド
を使い、羅列すれば充分。
void show_help()
{
Serial.println("? help");
Serial.println("f set finger degree");
Serial.println("w set wrist degree");
Serial.println("a set arm degree");
Serial.println("e set elbow degree");
Serial.println("b set body degree");
}
is_scmd
コマンド1文字が、サーボモータの角度を指定
するかどうかで、計算処理をするか否かを判定
します。
各関節のパラメータを指定するコマンドである
ときに、ONを返すようにします。
int is_scmd(char x) {
int result ;
// default
result = OFF ;
// judge
if ( x == 'f' ) { result = ON ; }
if ( x == 'w' ) { result = ON ; }
if ( x == 'a' ) { result = ON ; }
if ( x == 'e' ) { result = ON ; }
if ( x == 'b' ) { result = ON ; }
return result ;
}
get_value
バッファsbufの先頭にコマンド1文字、続けて
角度が10進数で入ってくるとします。
例) w123{enter}
sbufの添字1から最大3数字を取出して
数値に変換すればよいでしょう。
コマンドはパラメータを含めて'\r'で終了するので
これをターミネータとし、計算します。
int get_value() {
int result ;
char i ;
// default
result = 0 ;
// calculate
for ( i = 0 ; i < 3 ; i++ ) {
if ( sbuf[i+1] == '\r' ) break ;
result = result * 10 + (sbuf[i+1] - '0') ;
}
return result ;
}
コマンドインタプリタが出来れば、端末から
送信されてくる文字列を指定バッファに格納
する処理を考えます。
受信割込みで、文字列を指定バッファに格納
することにします。Serialでは、割込みを
serialEventという関数で定義します。
受信割込みハンドラは、次のように定義。
void serialEvent() {
char ch;
//
if ( Serial.available() > 0 ) {
// get 1 charactor
ch = Serial.read();
// store
sbuf[sindex] = ch ;
// increment
sindex++ ;
}
// judge
if ( ch == '\r' ) {
sindex = 0 ;
uflag = ON ;
}
}
コマンド、パラメータが格納されたかを
フラグuflagで通知します。
loopをまとめます。
void loop()
{
// serial handling
if ( uflag == ON ) {
uflag = OFF ;
// get command
cmd = sbuf[0] ;
// judge
if ( cmd == '?' ) { show_help() ; }
if ( is_scmd(cmd) == ON ) {
val = get_value() ;
if ( val > MAXANGLE ) { val = MAXANGLE ; }
switch ( cmd ) {
case 'f' : angleFinger = val ; break ;
case 'w' : angleWrist = val ; break ;
case 'a' : angleArm = val ; break ;
case 'e' : angleElbow = val ; break ;
case 'b' : angleBody = val ; break ;
}
}
}
// impress
finger.write(angleFinger);
wrist.write(angleWrist);
arm.write(angleArm);
elbow.write(angleElbow);
body.write(angleBody);
// delay
delay(xinterval);
}
setupは、Servoオブジェクトのピンアサインと
初期値の90度を設定しておきます。
void setup()
{
// serial
Serial.begin(9600);
// attaches the servo
finger.attach(Finger);
wrist.attach(Wrist);
arm.attach(Arm);
elbow.attach(Elbow);
body.attach(Body);
// place 90 degree
angleFinger = 90 ;
angleWrist = 90 ;
angleArm = 90 ;
angleElbow = 90 ;
angleBody = 90 ;
finger.write(angleFinger);
wrist.write(angleWrist);
arm.write(angleArm);
elbow.write(angleElbow);
body.write(angleBody);
// delay counter
xinterval = INTERV ;
// command interpreter buffer
uflag = OFF ;
sindex = 0 ;
}
スケッチに、まとめます。
#include <Servo.h>
#define OFF 0
#define ON OFF+1
// pin assignment
#define Finger 5
#define Wrist 6
#define Arm 9
#define Elbow 10
#define Body 11
#define MAXANGLE 180
#define INTERV 200
// create servo object to control servo motors
Servo finger ;
Servo wrist ;
Servo arm ;
Servo elbow ;
Servo body ;
char cmd ;
int xinterval ;
char uflag ;
char sbuf[8] ;
char sindex ;
int val ;
int angleFinger ;
int angleWrist ;
int angleArm ;
int angleElbow ;
int angleBody ;
void show_help()
{
Serial.println("? help");
Serial.println("f set finger degree");
Serial.println("w set wrist degree");
Serial.println("a set arm degree");
Serial.println("e set elbow degree");
Serial.println("b set body degree");
}
int get_value() {
int result ;
char i ;
// default
result = 0 ;
// calculate
for ( i = 0 ; i < 3 ; i++ ) {
if ( sbuf[i+1] == '\r' ) break ;
result = result * 10 + (sbuf[i+1] - '0') ;
}
return result ;
}
int is_scmd(char x) {
int result ;
// default
result = OFF ;
// judge
if ( x == 'f' ) { result = ON ; }
if ( x == 'w' ) { result = ON ; }
if ( x == 'a' ) { result = ON ; }
if ( x == 'e' ) { result = ON ; }
if ( x == 'b' ) { result = ON ; }
return result ;
}
void setup()
{
// serial
Serial.begin(9600);
// attaches the servo
finger.attach(Finger);
wrist.attach(Wrist);
arm.attach(Arm);
elbow.attach(Elbow);
body.attach(Body);
// place 90 degree
angleFinger = 90 ;
angleWrist = 90 ;
angleArm = 90 ;
angleElbow = 90 ;
angleBody = 90 ;
finger.write(angleFinger);
wrist.write(angleWrist);
arm.write(angleArm);
elbow.write(angleElbow);
body.write(angleBody);
// delay counter
xinterval = INTERV ;
// command interpreter buffer
uflag = OFF ;
sindex = 0 ;
}
void loop()
{
// serial handling
if ( uflag == ON ) {
uflag = OFF ;
// get command
cmd = sbuf[0] ;
// judge
if ( cmd == '?' ) { show_help() ; }
if ( is_scmd(cmd) == ON ) {
val = get_value() ;
if ( val > MAXANGLE ) { val = MAXANGLE ; }
switch ( cmd ) {
case 'f' : angleFinger = val ; break ;
case 'w' : angleWrist = val ; break ;
case 'a' : angleArm = val ; break ;
case 'e' : angleElbow = val ; break ;
case 'b' : angleBody = val ; break ;
}
}
}
// impress
finger.write(angleFinger);
wrist.write(angleWrist);
arm.write(angleArm);
elbow.write(angleElbow);
body.write(angleBody);
// delay
delay(xinterval);
}
// receive interrupt
void serialEvent() {
char ch;
if ( Serial.available() > 0 ) {
// get 1 charactor
ch = Serial.read();
// store
sbuf[sindex] = ch ;
// increment
sindex++ ;
}
// judge
if ( ch == '\r' ) {
sindex = 0 ;
uflag = ON ;
}
}
このスケッチを使うときは、ArduinoIDEの端末
エミュレータでは'\r'を入力できないので、
TeraTermを使って処理します。
目次
前
次