爪車

ヒト型ロボット 29 開発環境の整備・・・

先日、Code::Blocksをインストールし、使い始めたのですが、使用中に何度かフリーズに陥りました。
原因は不明です。ART-Linux化したUbuntu8.04なので、もしかしたらこれが原因かもしれません。たぶん関係ないと思うのですが。
仕方ないので、エデュタとコマンドラインで地道にプログラムを作成することにします。
単一のソースファイルなら問題なくビルドできるのですが、複数のファイルで構成するプログラムはどうしたらいいものか。makefile?なにそれ?な状態です。

Eclipse使いたい。PC買い換えたい。お金ない。

ヒト型ロボット 28 開発環境の整備

線形倒立振子のScilabでの検証もそこそこにして、プログラムの開発をC++ですることにしました。

現在は、Windowsでロボットのプログラム開発をしていますが、Linuxでの方が何かと便利なので、Linuxでの開発環境を整備することにしました。
プログラム開発はIDEでないとできない軟弱者なので、Visual C++に変わるものとして、Code::Blocksを導入してみました。
Linuxで使用できるC/C++統合開発環境はEclipseなど他にもいくつかあるのですが、Eclipseを試したところ、高機能なためか、Linux用に用意した中古PC(Pen4 2.4GHz)では動作が重かったです。

Code::Blocksのインストールは
http://demura.net/wordpress/9ode/547.htmlを参考にさせていただきました。
日本語化もできるようです。

ロボット開発ではLinuxが便利というよりむしろ必須だと思いますが、日常の生活で使うならWindowsのほうがやはり便利です。
ロボットの開発環境をLinuxに移行したいところですが、ルネサスのHEWなど、ロボット開発に必要な一部のソフトはLinuxでは動作しないので、全面移行は難しいです。

ヒト型ロボット 27 y軸も合成

線形倒立振子のy軸方向の運動も合成できました。
x軸と、y軸方向の運動は、着地点の設定が異なるだけで、後は同様の式で処理できます。
それぞれの軸の運動が計算できたら単純に合成するだけです。

xy平面に着地点と重心の運動をプロットしました。
軸の縦横比が違うので、実際よりy軸方向に伸びています。
t091118.gif

z軸方向も加えて、3次元のグラフで表示したいのですが、Scilabでのグラフ表示がまだ良くわからず、うまくプロットできませんでした。

ヒト型ロボット 26 線形倒立振子

「ヒューマノイドロボット」(オーム社)を読み進めて、Scilabで線形倒立振子のシミュレーションをしてみました。

x方向の2次元線形倒立振子のシミュレーション。着地点調整によるパターン生成です。
赤い点が目標着地点位置、緑の点が修正された着地点位置です。
zc = 0.8, Tsup = 0.8, 目標歩幅 0.3

t091117.gif

着地点を修正しているのが観察できます。歩行開始時には目標着地点とはだいぶ外れた点になっていますが、しだいに目標着地点に収束していきます。

本の通りに数式をプログラムしただけですが、うまくいきました。「ヒューマノイドロボット」を、はじめ読んだときは、さっぱり理解できなかったのですが、実際にプログラムして実行してみると、だいぶ理解できるようになりました。あとはy方向についても合成すれば3次元での運動になります。


ヒト型ロボット 25 とりあえずScilab導入

OctaveとScilabの両方をインストールして数日試してみただけですが、とりあえずScilabを導入することにしました。

倒立振子の簡単なシミュレーションをしてみました。
倒立振子

Scilabではコメントが//で書けるので、Octaveの#のコメントより入力が楽です。

ヒト型ロボット 24 数値計算ソフト

ぼちぼちとロボット制御プログラムの作成に取り組み始めました。

逆運動学などの数値計算をC言語で書いていましたが、行列の計算などが面倒なのと、自分で書いたプログラムだと計算値があまり信用できないので、数値計算ソフトを使ってみることにしました。
結局は、ロボットの制御プログラムはC/C++言語で書くことになるのですが、数値計算ソフトで動作確認してみます。
数値計算ソフトとしては、MathWorksのMATLABが有名ですが、値段が高いので購入するのは躊躇してしまいます。
そこでフリーのソフトを使うことにしましたが、OctaveとScilabのどちらを使うかで迷っています。
どちらもできることはほとんど同じなようですが・・・
とりあえず、両方試してみます。

SH7125でPWMサーボの制御

SH7125で16個のPWMサーボの制御です。
需要がありそうなので紹介します。

マイコンでPWMサーボを制御する方法はいろいろありますが、今回の方法は、タイマを使った割り込みで生成したパルスで、16個のPWMサーボを制御します。

けっこう前に書いたプログラムなので、自分でも何でこうなったか忘れてしまっているので、簡単な説明だけしておきます。

タイマはMTU2の0~4チャネルを同期動作で使用。
ポートEのPE0~PE15からパルス出力。
想定している制御周期は20ms。
TCNTオーバーフローで2.5msごとの割り込みが4回、その後10msで割り込み。
2.5msの割り込みで4個ずつまとめてパルスを立ち上げ。コンペアマッチで0出力。
割り込みで呼ばれるのはvoid MTU20_INT_OVF(void);

と言う感じです。

パルスの立ち上げは割り込みで 4つまとめて立ち上げています。立ち下げは割り込みではなく、ハードウェアでそれぞれ処理されるので、割り込みによるジッタの発生が抑えられていると思います。
ServoPuls[]にサーボの指令値を入れます。基準パルス幅1.5msは10進数で2344です。16進数だと0x928です。
秋月電子のAKI-7125を想定しています。
AKI-7125に搭載されているクリスタルは12.5MHzなので、MTU2クロックMPΦは25MHzで使用します。
アルファプロジェクトのSTK-7125などはクリスタルが10MHzなので、プログラムの修正が必要になります。
MPΦ=25MHz/16でカウントするので、1msは1562.5カウントになります。

以下のプログラムは実際のプログラムからの一部抜粋なので、このままでは動きません。
環境はHEW4です。

//プログラムここから

#include "iodefine.h"
#include <machine.h>

volatile unsigned int ServoPuls[16]; // サーボパルス

// main関数の一部
void main(void)
{
IO_init();
MTU2_init();
set_imask(0); // 割り込みマスクの解除

ServoPuls_init(); // ServoPuls初期化
MTU2_syn_start(); // タイマシンクロスタート

while (1) {
}
}

// ServoPulsの初期化
void ServoPuls_init(void)
{
int i;
for (i = 0; i < 16; i++) {
ServoPuls[i] = 0x928; // パルス1.5msにセット
}
}

// IO Portのセット
void IO_init(void)
{

// ------------------------------------------------------------------
// PortE サーボパルス用 [PE15:PE0] MTU2
// ------------------------------------------------------------------
PFC.PECRL4.BIT.PE15MD = 0x01; // PE15 is TIOC4D
PFC.PECRL4.BIT.PE14MD = 0x01; // PE14 is TIOC4C
PFC.PECRL4.BIT.PE13MD = 0x01; // PE13 is TIOC4B
PFC.PECRL4.BIT.PE12MD = 0x01; // PE12 is TIOC4A
PFC.PECRL3.BIT.PE11MD = 0x01; // PE11 is TIOC3D
PFC.PECRL3.BIT.PE10MD = 0x01; // PE10 is TIOC3C
PFC.PECRL3.BIT.PE9MD = 0x01; // PE9 is TIOC3B
PFC.PECRL3.BIT.PE8MD = 0x01; // PE8 is TIOC3A
PFC.PECRL2.BIT.PE7MD = 0x01; // PE7 is TIOC2D
PFC.PECRL2.BIT.PE6MD = 0x01; // PE6 is TIOC2C
PFC.PECRL2.BIT.PE5MD = 0x01; // PE5 is TIOC1B
PFC.PECRL2.BIT.PE4MD = 0x01; // PE4 is TIOC1A
PFC.PECRL1.BIT.PE3MD = 0x01; // PE3 is TIOC0D
PFC.PECRL1.BIT.PE2MD = 0x01; // PE2 is TIOC0C
PFC.PECRL1.BIT.PE1MD = 0x01; // PE1 is TIOC0B
PFC.PECRL1.BIT.PE0MD = 0x01; // PE0 is TIOC0A

PFC.PEIORL.WORD = 0xFFFF; // PE15-PE0 is output
}

// MTU2の初期化
void MTU2_init(void)
{
STB.CR4.BIT._MTU2 = 0; // モジュールスタンバイの解除
MTU2.TSTR.BYTE = 0x00; // タイマ0~4停止
MTU2.TRWER.BIT.RWE = 1; // 3,4のレジスタのリードライトを許可する
MTU2.TSYR.BYTE = 0xC7; // TCNT0~4同期動作

// タイマ0~4同期動作
MTU20.TCR.BYTE = 0x62; // 同期クリア、MPφ/16 1count -> 6.4*10^(-7)s 1ms -> 1562.5count
MTU21.TCR.BYTE = 0x62; // 同期クリア、MPφ/16 1count -> 6.4*10^(-7)s 1ms -> 1562.5count
MTU22.TCR.BYTE = 0x62; // 同期クリア、MPφ/16 1count -> 6.4*10^(-7)s 1ms -> 1562.5count
MTU23.TCR.BYTE = 0x62; // 同期クリア、MPφ/16 1count -> 6.4*10^(-7)s 1ms -> 1562.5count
MTU24.TCR.BYTE = 0x62; // 同期クリア、MPφ/16 1count -> 6.4*10^(-7)s 1ms -> 1562.5count

MTU20.TMDR.BYTE = 0x00; // タイマ通常動作
MTU21.TMDR.BYTE = 0x00;
MTU22.TMDR.BYTE = 0x00;
MTU23.TMDR.BYTE = 0x00;
MTU24.TMDR.BYTE = 0x00;

MTU20.TIER.BIT.TCIEV = 1; // OVFによる割り込み許可
MTU21.TIER.BIT.TCIEV = 1; // OVFによる割り込み許可
MTU22.TIER.BIT.TCIEV = 1; // OVFによる割り込み許可
MTU23.TIER.BIT.TCIEV = 1; // OVFによる割り込み許可
MTU24.TIER.BIT.TCIEV = 1; // OVFによる割り込み許可

INTC.IPRD.BIT._MTU20C = 15; // MTU20 OVF 割り込み優先度 15

MTU2.TOER.BYTE = 0xFF; // 3,4の出力許可

MTU20.TIOR.BIT.IOA = 0x00; // TGRA_0 PE0 コンペアマッチで出力保持
MTU20.TIOR.BIT.IOB = 0x00; // TGRB_0 PE1 コンペアマッチで出力保持
MTU20.TIOR.BIT.IOC = 0x00; // TGRC_0 PE2 コンペアマッチで出力保持
MTU20.TIOR.BIT.IOD = 0x00; // TGRD_0 PE3 コンペアマッチで出力保持
MTU21.TIOR.BIT.IOA = 0x00; // TGRA_1 PE4 コンペアマッチで出力保持
MTU21.TIOR.BIT.IOB = 0x00; // TGRB_1 PE5 コンペアマッチで出力保持
MTU22.TIOR.BIT.IOA = 0x00; // TGRA_2 PE6 コンペアマッチで出力保持
MTU22.TIOR.BIT.IOB = 0x00; // TGRB_2 PE7 コンペアマッチで出力保持
MTU23.TIOR.BIT.IOA = 0x00; // TGRA_3 PE8 コンペアマッチで出力保持
MTU23.TIOR.BIT.IOB = 0x00; // TGRB_3 PE9 コンペアマッチで出力保持
MTU23.TIOR.BIT.IOC = 0x00; // TGRC_3 PE10 コンペアマッチで出力保持
MTU23.TIOR.BIT.IOD = 0x00; // TGRD_3 PE11 コンペアマッチで出力保持
MTU24.TIOR.BIT.IOA = 0x00; // TGRA_4 PE12 コンペアマッチで出力保持
MTU24.TIOR.BIT.IOB = 0x00; // TGRB_4 PE13 コンペアマッチで出力保持
MTU24.TIOR.BIT.IOC = 0x00; // TGRC_4 PE14 コンペアマッチで出力保持
MTU24.TIOR.BIT.IOD = 0x00; // TGRD_4 PE15 コンペアマッチで出力保持

MTU20.TGRA = 0xF0BD + 0x928; // コンペアマッチのセット
MTU20.TGRB = 0xF0BD + 0x928;
MTU20.TGRC = 0xF0BD + 0x928;
MTU20.TGRD = 0xF0BD + 0x928;
MTU21.TGRA = 0xF0BD + 0x928;
MTU21.TGRB = 0xF0BD + 0x928;
MTU22.TGRA = 0xF0BD + 0x928;
MTU22.TGRB = 0xF0BD + 0x928;
MTU23.TGRA = 0xF0BD + 0x928;
MTU23.TGRB = 0xF0BD + 0x928;
MTU23.TGRC = 0xF0BD + 0x928;
MTU23.TGRD = 0xF0BD + 0x928;
MTU24.TGRA = 0xF0BD + 0x928;
MTU24.TGRB = 0xF0BD + 0x928;
MTU24.TGRC = 0xF0BD + 0x928;
MTU24.TGRD = 0xF0BD + 0x928;

MTU20.TCNT = 0x0;

MTU20.TSR.BIT.TCFV = 0; // OVFフラグクリア

}

// MTU20 OVF 割り込み
// intprg.cで
//// 92 MTU2_0 TCIV0
//void INT_MTU2_0_TCIV0(void)
//{
// MTU20_INT_OVF();
//}
void MTU20_INT_OVF(void)
{
volatile static int count = 0;

MTU2.TSTR.BYTE = 0x00; // タイマ0~4停止
MTU20.TSR.BIT.TCFV = 0; // OVFフラグクリア

switch (count) {
case 0:
MTU20.TCNT = 0xF0BD; // 2.5ms セット

MTU20.TIOR.BIT.IOA = 0x05; // TGRA_0 PE0 初期1出力コンペアマッチで0出力
MTU20.TIOR.BIT.IOB = 0x05; // TGRB_0 PE1 初期1出力コンペアマッチで0出力
MTU20.TIOR.BIT.IOC = 0x05; // TGRC_0 PE2 初期1出力コンペアマッチで0出力
MTU20.TIOR.BIT.IOD = 0x05; // TGRD_0 PE3 初期1出力コンペアマッチで0出力

MTU2.TCSYSTR.BYTE = 0xF8; // 0,1,2,3,4 タイマシンクロスタート
count++;
break;
case 1:
MTU20.TCNT = 0xF0BD; // 2.5ms セット

MTU21.TIOR.BIT.IOA = 0x05; // TGRA_1 PE4 初期1出力コンペアマッチで0出力
MTU21.TIOR.BIT.IOB = 0x05; // TGRB_1 PE5 初期1出力コンペアマッチで0出力
MTU22.TIOR.BIT.IOA = 0x05; // TGRA_2 PE6 初期1出力コンペアマッチで0出力
MTU22.TIOR.BIT.IOB = 0x05; // TGRB_2 PE7 初期1出力コンペアマッチで0出力

MTU2.TCSYSTR.BYTE = 0xF8; // 0,1,2,3,4 タイマシンクロスタート
count++;
break;
case 2:
MTU20.TCNT = 0xF0BD; // 2.5ms セット

MTU23.TIOR.BIT.IOA = 0x05; // TGRA_3 PE8 初期1出力コンペアマッチで0出力
MTU23.TIOR.BIT.IOB = 0x05; // TGRB_3 PE9 初期1出力コンペアマッチで0出力
MTU23.TIOR.BIT.IOC = 0x05; // TGRC_3 PE10 初期1出力コンペアマッチで0出力
MTU23.TIOR.BIT.IOD = 0x05; // TGRD_3 PE11 初期1出力コンペアマッチで0出力

MTU2.TCSYSTR.BYTE = 0xF8; // 0,1,2,3,4 タイマシンクロスタート
count++;
break;
case 3:
MTU20.TCNT = 0xF0BD; // 2.5ms セット

MTU24.TIOR.BIT.IOA = 0x05; // TGRA_4 PE12 初期1出力コンペアマッチで0出力
MTU24.TIOR.BIT.IOB = 0x05; // TGRB_4 PE13 初期1出力コンペアマッチで0出力
MTU24.TIOR.BIT.IOC = 0x05; // TGRC_4 PE14 初期1出力コンペアマッチで0出力
MTU24.TIOR.BIT.IOD = 0x05; // TGRD_4 PE15 初期1出力コンペアマッチで0出力

MTU2.TCSYSTR.BYTE = 0xF8; // 0,1,2,3,4 タイマシンクロスタート
count++;
break;
default:
MTU20.TCNT = 0x3D09; // 10ms セット

MTU2.TCSYSTR.BYTE = 0xF8; // 0,1,2,3,4 タイマシンクロスタート

// servo puls set
MTU20.TGRA = 0xF0BD + ServoPuls[0];
MTU20.TGRB = 0xF0BD + ServoPuls[1];
MTU20.TGRC = 0xF0BD + ServoPuls[2];
MTU20.TGRD = 0xF0BD + ServoPuls[3];
MTU21.TGRA = 0xF0BD + ServoPuls[4];
MTU21.TGRB = 0xF0BD + ServoPuls[5];
MTU22.TGRA = 0xF0BD + ServoPuls[6];
MTU22.TGRB = 0xF0BD + ServoPuls[7];
MTU23.TGRA = 0xF0BD + ServoPuls[8];
MTU23.TGRB = 0xF0BD + ServoPuls[9];
MTU23.TGRC = 0xF0BD + ServoPuls[10];
MTU23.TGRD = 0xF0BD + ServoPuls[11];
MTU24.TGRA = 0xF0BD + ServoPuls[12];
MTU24.TGRB = 0xF0BD + ServoPuls[13];
MTU24.TGRC = 0xF0BD + ServoPuls[14];
MTU24.TGRD = 0xF0BD + ServoPuls[15];

count = 0;

break;
}
}

// タイマ0~4シンクロスタート
void MTU2_syn_start(void)
{
MTU2.TCSYSTR.BYTE = 0xF8; // タイマ0~4シンクロスタート
}

// タイマ0~4停止
void MTU2_syn_stop(void)
{
MTU2.TSTR.BYTE = 0x00; // タイマ0~4停止
}

// プログラムここまで

ヒト型ロボット 23 USB-シリアル変換

ART-Linux化したUbuntu 8.04でUSB-シリアル変換のFT232を使ってシリアル通信してみました。

ART-Linuxを入れたPCは中古で買った物で、シリアルポートも付いていたのですが、OSから認識されず、使えなかったので、ストロベリーリナックスUSBシリアル変換モジュールキット「FT232RX」 を使ってみました。

ART-Linux化したので、ドライバ関係が心配だったのですが、接続しただけで仮想COMポートとして認識されました。

認識されているのかの確認は端末から
$ dmesg
で確認できます。
/dev/ttyUSB0
として認識されてました。

とりあえず、非リアルタイム処理での通信の確認をしてみました。

WindowsでいうTeraTermに相当するものとしてcutecomを使用してみました。
cutecomはGUIのターミナルソフトです。
デバイスの選択のところの一覧には/dev/ttyUSB0がないので、自分で書き加えます。
FT232RXのTXDとRXDのピンをジャンパーピンでつないでループバックテストをしてみました。
600~921600bpsの各種Baud rateで通信できました。
非リアルタイム処理でなら問題なく使用できるみたいです。

リアルタイム処理でも自由に通信できれば、PCからロボットを自由に制御できるようになります。

ArduinoでICS2.0制御の実験

Arduinoで近藤科学のICS2.0対応サーボ(KRS-4014)をシリアル制御してみました。

コマンドを送信してひとつのサーボを動かすだけの実験です。
一応うまくいきましたが、ArduinoでのICS2.0制御はあまりお勧めできないので、参考程度にしてください。
実際に実験される際は十分に注意してください。

この実験で使用したArduinoのバージョンはArduino-0016で、ArduinoボードはATMEGA 328を搭載しているArduino Duemilanoveです。
シリアル通信の設定をする際に、ATMEGA 328のレジスタを直接操作しているので、これ以外の環境では動作しないと思います。
Arduinoでは、レジスタの直接の操作は、さまざまな問題を引き起こすので推奨されませんが、通常のArduino言語ではできないことなので仕方なくです。

・Arduinoとサーボの接続
単線双方向シリアルの接続回路はベストテクノロジーさんのブログの
ATmega128マイコンボードとAXモータとの接続#1
ATmega128マイコンボードとAXモータとの接続#2
を参考にしました。

紹介されている回路をまとめるとこんな感じになります。

74HC241
TC74HC241AP
DIR-CTRLは送受信切替です。Hレベルで送信、Lレベルで受信となります。

実際にArduinoと接続するとこんな感じです。
DSCN0936.jpg
今回は信号の送信の実験だけなのでDIR-CTRLはHレベルに固定しています。
送受信の切替をするならばDIR-CTRLをマイコンで制御します。
サーボへの電源は外部の安定化電源を用いています。
Arduino、サーボ、外部電源のグランドは忘れずに接続します。信号線だけサーボに接続しても動きません。

・Arduinoのスケッチ
//Arduino 0016
// ID:0のサーボを20ms周期で制御

// スケッチここから

#define SERVO_MIN 3500 // サーボ最小動作角(-135度)
#define SERVO_CENTER 7500 // サーボ中心角(0度)
#define SERVO_MAX 11500 // サーボ最大動作角(+135度)

float theta = 0.0;

void setup() {
Serial.begin(115200);
// USART動作設定 非同期動作 ビット長8bit ストップ1bit パリティEVEN(偶数)
UCSR0C = (0<<UMSEL00) | (3<<UCSZ00) | (0<<USBS0) | (1<<UPM01) | (0<<UPM00);
}

void ics2_set_pos(unsigned int id, unsigned int position) {
byte cmd, pos_h, pos_l;

// CMD コマンドとID
cmd = (byte)(128 + id);

// POS サーボの設定舵角
pos_h = (byte)(position / 128); // 上位7bit
pos_l = (byte)(position % 128); // 下位7bit

// 1バイトのデータとして送信
Serial.print(cmd, BYTE);
Serial.print(pos_h, BYTE);
Serial.print(pos_l, BYTE);

}

void loop() {
ics2_set_pos(0, SERVO_CENTER + (unsigned int)(1000*sin(theta)));
theta += 0.05;
delay(20); // 20ms一時停止
}

// スケッチここまで

ICS2.0通信するためシリアル通信の設定をします。
ICS2.0の通信条件は
通信速度 115200bps
ビット長 8bit
スタート 1bit
ストップ 1bit
フロー制御 無し
パリティ EVEN(偶数)
です。
これらの設定は通常のArduino言語ではできません。
そこでATmega328のレジスタを直接操作して設定することになります。
// USART動作設定 非同期動作 ビット長8bit ストップ1bit パリティEVEN(偶数)
UCSR0C = (0<<UMSEL00) | (3<<UCSZ00) | (0<<USBS0) | (1<<UPM01) | (0<<UPM00);

このレジスタ操作はArduinoボードに搭載されているマイコンがATMEGA328の場合です。

Arduinoボードへスケッチをアップロードする際は回路を接続しないでください。接続したままだと正しくアップロードされません。

アップロード後に回路を接続して、サーボを動作させてください。
うまくいけばサーボが動作するはずです。
サーボがPWMモードになっていると動かないことがあるので、事前に近藤科学純正のICS USBアダプターを用いてPWMモードをオフにしておいたほうがいいです。

コマンドの送信後に、サーボから返信がありますので、続けてコマンドを送る際は適切なウエイトを入れます。最低でも1msは必要です。

Top

HOME

tsumehashi

Author:tsumehashi
FC2ブログへようこそ!

10 | 2009/11 | 12
1 2 3 4 5 6 7
8 9 10 11 12 13 14
15 16 17 18 19 20 21
22 23 24 25 26 27 28
29 30 - - - - -