 |
 |
 |
 |
プログラミング 
前進したり、相手を探したり、土俵際に来たらバックしたりする相撲ロボットのプログラムを作成します。
処理の流れを図にする。
地面が無くなったら後退して旋回します。相手を見つけたら全速で直進します。プッシュボタンでプログラムを終了します。
初期化
GCC Developer Liteを起動し、初期化部分を記述します。
fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP); fd_SetUVThreshold (7.4); DX_ChangeBaudrate (1000000);
AX-12Aをエンドレスターンモードに切替える
AX-12Aは、通常特定の位置へ移動する位置決めモードになっています。位置決めモードは0~300度の制限があり、車輪のように回転させることはできません。
AX-12Aには車輪と同様に無限に回転させるためのエンドレスターンモードが用意されています。
fd_DXSetEndlessTurn (1, true); fd_DXSetEndlessTurn (2, true);
fd_DXSetEndlessTurn
指定されたIDのアクチュエータのモードを切り替えます。モード切替時にEEPROM(不揮発性メモリ)内の書換えが発生します。EEPROMの書換え回数は数万回ですが、ループ内でモードの切替が発生しているとすぐに限界を迎えることになります。
第1引数は、DynamixelのIDを指定します。
第2引数は、モードの指定を行います。trueはエンドレスターンモード、falseは位置決めモードです。
一度エンドレスターンモードに設定すると、電源を切っても設定は保存されます。位置決めモードを使用する際は、必ずfd_DXSetEndlessTurnで位置決めモードに設定して下さい。
AS-S1の赤外線反射センサの値を取得する
地面に向けた右側と正面の赤外線反射センサの値を取得します。
uint8_t EnemyData, GroundData;
while (!fd_GetPB ()) {
DX_ReadByteData (100, 27, &EnemyData, 10, NULL); // 正面センサ値取得
DX_ReadByteData (100, 28, &GroundData, 10, NULL); // 右側センサ値取得
fd_printf ("enemy: %3d ground: %3d\r", EnemyData, GroundData);
fd_Wait (10);
}
AX-S1の右側は地面を向いているので、GroundDataは常に255になります。もし、AX-S1を反対に取り付けていた場合は、左側のセンサ値を取得して下さい。
AX-12Aを回転させる
エンドレスターンモードでは位置ではなく、速度を指定することで回転を行います。
// アクチュエータ操作
void Direction (short left, short right) {
fd_DXSetSpeed (1, -right);
fd_DXSetSpeed (2, left);
}
ID1、ID2のAX-12Aは逆向きに取り付けていますので、値を反転させる必要があります。個別に記述しても良いのですが、間違えないように関数として1つにまとめてみましょう。
ここではID1が右車輪を想定しているため、値を-(マイナス)しています。AX-12Aの取り付けが反対であれば、ID2の値を-(マイナス)指定して下さい。
fd_DXSetSpeed
指定したIDのアクチュエータの速度を指定します。
第1引数は、DynamixelのIDです。
第2引数は、位置指定モードの場合は角速度(単位は約0.111rpm)ですが、エンドレスターンモードの場合は最大出力に対する出力の割合になります。
センサ値に応じて方向を変える
右側のセンサで土俵際を検知したら2秒間後退し、左に旋回します。
正面のセンサで相手を見つけたら、100%の出力で直進します。見つからない場合は出力を50%に落として直進します。
uint8_t EnemyData, GroundData;
fd_DXSetEndlessTurn (1, true);
fd_DXSetEndlessTurn (2, true);
while (!fd_GetPB ()) {
DX_ReadByteData (100, 27, &EnemyData, 10, NULL);
DX_ReadByteData (100, 28, &GroundData, 10, NULL);
fd_printf ("enemy: %3d ground: %3d\r", EnemyData, GroundData);
if (GroundData < 200) { // 土俵際検知
Direction (-512, -512); // 後退
fd_Wait (2000);
Direction (-512, 512); // 旋回
fd_Wait (2000);
} else {
if (EnemyData > 30) { // 正面敵検知
Direction (1023, 1023); // 全速前進
} else {
Direction (512, 512); // 50%で前進
}
}
fd_Wait (10);
}
プログラム完成
ではプログラムをまとめてみましょう。
#include <fd.h>
// アクチュエータ操作
void Direction (short left, short right) {
fd_DXSetSpeed (1, -right);
fd_DXSetSpeed (2, left);
}
// メイン関数
void main (void) {
uint8_t EnemyData, GroundData;
fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);
fd_SetUVThreshold (7.4);
DX_ChangeBaudrate (1000000);
fd_DXSetEndlessTurn (1, true);
fd_DXSetEndlessTurn (2, true);
while (!fd_GetPB ()) {
DX_ReadByteData (100, 27, &EnemyData, 10, NULL);
DX_ReadByteData (100, 28, &GroundData, 10, NULL);
fd_printf ("enemy: %3d ground: %3d\r", EnemyData, GroundData);
if (GroundData < 200) { // 土俵際検知
Direction (-512, -512); // 後退
fd_Wait (2000);
Direction (-512, 512); // 旋回
fd_Wait (2000);
} else {
if (EnemyData > 30) { // 正面敵検知
Direction (1023, 1023); // 全速前進
} else {
Direction (512, 512); // 50%で前進
}
}
fd_Wait (10);
}
Direction (0, 0); // 停止
}
作成したDirection関数でいろいろな動きが可能になります。
Direction (100, 512); // 左へ曲がる Direction (512, -512); // その場で右に曲がる(右旋回) Direction (-512, -100); // 後退しながら右に曲がる
flow.png
