11: 2011-08-09 (火) 20:18:16 yoshida  |
12: 2012-02-20 (月) 14:39:53 yoshida |
| | #include <fd.h> | | #include <fd.h> |
| | void main (void) { | | void main (void) { |
| - | uint8_t ir; | + | uint8_t ir = 0; |
| | + | |
| | + | fd_Wait (2000); |
| | // 初期化 | | // 初期化 |
| | fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP); | | fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP); |
| | fd_SetUVThreshold (7.4); | | fd_SetUVThreshold (7.4); |
| | DX_ChangeBaudrate (1000000); | | DX_ChangeBaudrate (1000000); |
| - | ir = 0; // 距離初期化 | + | |
| - | // 出力制限(20%) | + | // 状態確認 |
| - | fd_DXSetTorqueLimit (1, 205); | + | fd_printf("Present Voltage[x10] = %d\n", fd_GetPresentVoltage ()); |
| - | // ID1のDynamixelが存在すること | + | fd_printf("Ping to AX-12 = %s\n", DX_Ping (1, 500, NULL) ? "OK" : "NG"); |
| - | if (DX_Ping (1, 100, NULL)) { | + | fd_printf("Ping to AX-S1 = %s\n", DX_Ping (100, 500, NULL) ? "OK" : "NG"); |
| - | // 何かキーボード入力があるまで繰り返す | + | |
| - | while (!fd_rx_buff()) { | + | // 出力制限(50%) |
| - | // AX-S1より赤外線センサ(正面)の距離検出 | + | fd_DXSetTorqueLimit (1, 511); |
| - | if (DX_ReadByteData (100, 27, &ir, 50, NULL)) { | + | // 無限回転モードOFF |
| - | fd_printf ("%3d\r", ir); | + | fd_DXSetEndlessTurn (1, false); |
| - | if (ir >100) { | + | |
| - | // Dynamixelのポジション書込み | + | // コンソールからの入力があるまで繰り返す |
| - | fd_DXSetPosition (1, 300); | + | while (!fd_rx_buff()) { |
| - | fd_Wait (2000); | + | // AX-S1より赤外線センサ(正面)の反射率計測 |
| - | } else { | + | if (DX_ReadByteData (100, 27, &ir, 50, NULL)) { |
| - | fd_DXSetPosition (1, 512); | + | fd_printf ("\rAX-S1 Front IR = %3d\33[K", ir); |
| - | } | + | if (ir > 100) { |
| | + | // Dynamixelのポジション書込み |
| | + | fd_DXSetPosition (1, 300); |
| | + | } else { |
| | + | // Dynamixelのポジション書込み |
| | + | fd_DXSetPosition (1, 512); |
| | } | | } |
| - | fd_Wait (1); | + | } else { |
| | + | fd_printf ("\rRead from AX-S1:NG\33[K", ir); |
| | } | | } |
| | + | // 50ミリ秒の待ち |
| | + | fd_Wait (50); |
| | } | | } |
| | + | // 位置決め制御停止 |
| | + | fd_DXEnableControl (1, false); |
| | } | | } |
| | </pre> | | </pre> |
ページへ戻る
