サーボ駆動プログラムの作成

概要

armadillo300のシリアルデバイス経由でプログラムによりサーボを動作させる．ここで，armadillo300のシリアルデバイスに接続する機器として以下のものがある．

• armadillo300のシリアルデバイス
  • RS-232Cレベルの信号を扱うシリアルデバイスである．具体的には，-15V～15Vの範囲を扱い，論理0が+3V～+15V，論理1が-15V～-3Vである．
• AGB65-RSC
  • シリアルポート経由で受けた指令を元にサーボを制御する機器である．この機器が扱うシリアルデバイスはTTLレベル信号であり，論理0が0V，論理1が5Vである．
• AGB65-232C
  • RS-232Cレベルの信号とTTLレベルの信号には電圧互換がないため，変換処理が必要となる．AGB65-232CはRS-232Cレベルの信号とTTLレベルの信号の変換処理を行う機器である．
• サーボモータ
  • 物体の位置などを制御量として目標値に追従するように自動で作動するモータ．

各デバイスの接続は図1のようになる

図1. 各デバイス接続図

実験前

準備・確認

1. 全ての電源が入っていることを確認
2. TeraTermでarmadillo300にログインできていることを確認
3. 演習用のディレクトリを作成し，そのディレクトリに移動していることを確認

• 演習後は消すこと
• 演習用ディレクトリにて，プログラムファイルを作成し，コンパイル，実行を行うこと．
  • コンパイル例:プログラムファイル:sample.c, 実行ファイル:sample
  • gcc sample.c -o sample
• 実行
  • ./sample

AGB65-RSC簡易使用方法

以下のように配列を定義して値を代入する.

unsigned char output[7] = {255,3,4,2,0,127,20};

各配列の値の意味は以下のようになる．

簡易使用時の配列定義

シンクロバイト(255)	ＲＳＣにデータの通信開始を知らせるデータで,常に「２５５」で始まる.
ＩＤ(3)	ＲＳＣに設定された固有のＩＤ(AGB65シリーズを複数接続したときの判別用.ＲＳＣの場合,出荷時のＩＤは「３」.)
送信バイト数(4)	送信される命令の（バイト）数.シンクロバイト,ＩＤ,送信バイト数は数えない.
命令1(2)	個別サーボ駆動設定
命令2(0)	サーボ番号.0はRSC基板上のP0に対応
命令3(127)	角度(min:0,max:255で,絶対位置を指定)
命令4(2)	指定した角度まで移動する速度(x15ms)

この配列をシリアル送信する.

AGB65-RSC命令値とサーボ角度の関係について

命令値の範囲

• 0～255

命令値に対するサーボの可動角

• 「だいたい」0°～180°(0°の位置はサーボによる)

プログラミング演習

下記のプログラムを作成，実行してサーボの制御について学ぶ．

対話的サーボ制御プログラム

入力 in: 出力 out

• 0 ≦ in ≦ 255 : inに応じたサーボの角度
• 上記以外 : プログラム終了

#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define SERIAL_PORT     "/dev/ttyAM1"

int main(){
  int fd;
  fd = open(SERIAL_PORT, O_RDWR | O_NOCTTY);
  if(fd < 0){
    printf("%s doesn't open it\n",SERIAL_PORT);
    return -1;
  }

  struct termios oldtio, newtio;
  tcgetattr(fd, &oldtio);
  newtio = oldtio;

  /* 通信方式の設定・非カノニカル入力処理選択 */
  newtio.c_iflag = IGNPAR;
  newtio.c_lflag = 0;

  /* read関数での文字待ちうけの設定 */
  newtio.c_cc[VTIME] = 0; /* 値x0.1秒待つ */
  newtio.c_cc[VMIN] = 1; /* 値の文字分だけ入力されるまで待つ*/

  /* 通信速度の設定 */
  cfsetspeed(&newtio, B9600);

  /* シリアルデバイス初期化処理 */
  tcflush(fd, TCIFLUSH);
  tcsetattr(fd, TCSANOW, &newtio);

  /* サーボ指令の雛形 */
  /* 個別サーボ駆動モード, サーボコントローラID:3, サーボ番号:0 */
  unsigned char output[7] = {255,3,4,2,0,127,20};

  /* input(入力):サーボへの角度指令:有効範囲0(0°)-255(180°) */
  /* 入力が有効範囲内の間，サーボをコントロール */
  int input;
  while(1){
    printf("input value from 0 to 255[-1:end]:");
    scanf("%d", &input);
    if(input < 0 || input > 255){ /* サーボコントロール終了 */
      break;
    }
    /* output[5]がサーボへの角度指令 */
    output[5] = (unsigned char)input;
    write(fd, output, sizeof(output));
  }

  /* シリアルデバイス終了処理 */
  tcsetattr(fd, TCSANOW, &oldtio);
  close(fd);
  return 0;
}

対話的メータ表示プログラム

値を入力するとメーター上の対応する値を指すようにサーボモータを制御するプログラムを作成する

• ポイント:メーター上の数値に合うように入力値を変換する必要がある.

参考情報

AGB65-RSC命令仕様

• マスタ(Armadillo-300)からRSCへの指示は,４～１７バイトの数値データで送る.
• バイトサイズ（８ビット）なので,表される数値は０～２５５の２５６種類.
• 数値は１０進数.
• ** 注）データは文字データではなく,数値（バイナリ）データで送る. **

データの基本形

データの基本形は次の通り.[　]内は１バイトを表す.（）内は送りえる数値の範囲.

［シンクロバイト(255)]　[ＩＤ(0-3)］　[送信バイト数(1-14)］　［命令１]　[命令２]　[命令３]　．．

この時の各意味は以下の表となる．

データ各部位の説明

シンクロバイト	ＲＳＣにデータの通信開始を知らせるデータで,常に「２５５」で始まる.
ＩＤ	ＲＳＣに設定された固有のＩＤ(AGB65シリーズを複数接続したときの判別用.ＲＳＣの場合,出荷時のＩＤは「３」.)
送信バイト数	送信される命令の（バイト）数.シンクロバイト,ＩＤ,送信バイト数は数えない.
命令	ＲＳＣに動作させたい命令.詳細は以下の表.

命令の種類は以下のものがある．

命令の種類

命令値	動作	方向	フォーマット
1	全サーボ駆動	送信	[255][ID][長(14)][命令(1)][P0][P1]...[P10][P11][Speed]
2	個別サーボ駆動	送信	[255][ID][長(4)][命令(2)][サーボ番号][サーボ位置][Speed]
3	全サーボパルス停止	送信	[255][ID][長(1)][命令(3)]
4	指定サーボパルス停止	送信	[255][ID][長(2)][命令(4)][サーボ番号]
5	180°モード	送信	[255][ID][長(1)][命令(5)]
6	255解像度モード(default)	送信	[255][ID][長(1)][命令(6)]

（＊）[Speed]は指定したサーボ位置までにかかる時間.[Speed]×15ms.