前進したり、相手を探したり、土俵際に来たらバックしたりする相撲ロボットのプログラムを作成します。
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は、通常特定の位置へ移動する位置決めモードになっています。位置決めモードは0~300度の制限があり、車輪のように回転させることはできません。
AX-12Aには車輪と同様に無限に回転させるためのエンドレスターンモードが用意されています。
fd_DXSetEndlessTurn (1, true); fd_DXSetEndlessTurn (2, true);
指定されたIDのアクチュエータのモードを切り替えます。モード切替時にEEPROM(不揮発性メモリ)内の書換えが発生します。EEPROMの書換え回数は数万回ですが、ループ内でモードの切替が発生しているとすぐに限界を迎えることになります。
第1引数は、DynamixelのIDを指定します。
第2引数は、モードの指定を行います。trueはエンドレスターンモード、falseは位置決めモードです。
一度エンドレスターンモードに設定すると、電源を切っても設定は保存されます。位置決めモードを使用する際は、必ずfd_DXSetEndlessTurnで位置決めモードに設定して下さい。
地面に向けた右側と正面の赤外線反射センサの値を取得します。
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を反対に取り付けていた場合は、左側のセンサ値を取得して下さい。
エンドレスターンモードでは位置ではなく、速度を指定することで回転を行います。
// アクチュエータ操作 void Direction (short left, short right) { fd_DXSetSpeed (1, -right); fd_DXSetSpeed (2, left); }
ID1、ID2のAX-12Aは逆向きに取り付けていますので、値を反転させる必要があります。個別に記述しても良いのですが、間違えないように関数として1つにまとめてみましょう。
ここではID1が右車輪を想定しているため、値を-(マイナス)しています。AX-12Aの取り付けが反対であれば、ID2の値を-(マイナス)指定して下さい。
指定した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); // 後退しながら右に曲がる
(This host) = http://www.besttechnology.co.jp