11: 2011-08-09 (火) 20:18:16 yoshida[6] [7] | 12: 2012-02-20 (月) 14:39:53 yoshida[6] [8] | ||
---|---|---|---|
Line 57: | Line 57: | ||
#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> |
(This host) = http://www.besttechnology.co.jp