Diff: main.cpp

Revision:

1:ef646c52e08f

Parent:

0:50f3665aec00

Child:

2:7004e3d0281f

--- a/main.cpp	Mon Jan 14 07:50:04 2013 +0000
+++ b/main.cpp	Mon Jan 14 16:25:33 2013 +0000
@@ -1,21 +1,37 @@
 #include "mbed.h"
 #include "herkulex.h"
 
+// set serial port and baudrate, (mbed <-> HerculexX)
 Herkulex sv(p9, p10, 115200);
-DigitalOut led(LED1);
 
 int main() 
 {   
-    wait(1);
+    sv.clear(0xFD);
     sv.setTorque(0xFD, TORQUE_ON);
+    
     while(1) 
-    {
-        led=1;
-        sv.movePos(0xFD, 1002, 100, SET_MODE_POS, SET_LED_GREEN_ON);
-        wait(3);
+    {       
+        // POS Mode, +159.8 Degree, 100*11.2ms = 1120ms, Green LED On
+        sv.movePos(0xFD, 1002, 100, GLED_ON);
+        wait(5);
         
-        led=0;       
-        sv.movePos(0xFD, 21, 100, SET_MODE_POS, SET_LED_BLUE_ON);
-        wait(3);
+        // POS Mode, -159.8 Degree, 100*11.2ms = 1120ms, Blue LED On
+        sv.movePos(0xFD, 21, 100, BLED_ON);
+        wait(5);
+
+        // TURN Mode, CCW Turn, Green LED On
+        sv.turn(0xFD, 1023, GLED_ON);
+        wait(5);
+        
+        // TURN Mode, CW Turn, Blue LED On        
+        sv.turn(0xFD, -1023, BLED_ON);
+        wait(5);
+        
+        // POS Mode, 0 Degree, 100*11.2ms = 1120ms, Red LED On
+        sv.movePos(0xFD, 512, 100, RLED_ON);
+        wait(5);
+        
+        // Clear error status
+        sv.clear(0xFD);
     }
 }