herkulex servo control library

Dependents:   HerkuleX-HelloWorld

This herkulex library is based on DongBu Robot documentation and protocol.

http://dasarobot.com/guide/herkulexeng.pdf

/media/uploads/passionvirus/mbedandherkulex_i.png /media/uploads/passionvirus/range.png

Files at this revision

API Documentation at this revision

Comitter:
passionvirus
Date:
Mon Jan 14 21:03:43 2013 +0000
Parent:
5:f737e5c70115
Commit message:
add function of getPos, getStatus, rxPacket etc.

Changed in this revision

herkulex.cpp Show annotated file Show diff for this revision Revisions of this file
herkulex.h Show annotated file Show diff for this revision Revisions of this file
--- a/herkulex.cpp	Mon Jan 14 16:24:05 2013 +0000
+++ b/herkulex.cpp	Mon Jan 14 21:03:43 2013 +0000
@@ -28,7 +28,7 @@
     #ifdef HERKULEX_DEBUG
         pc = new Serial(USBTX, USBRX);
         pc->baud(57600);
-        pc->printf("Herkulex Init!\n");
+        pc->printf("\n\nHerkulex Init!\n");
     #endif
     
     txd = new Serial(tx, NC);
@@ -74,6 +74,27 @@
 }
 
 //------------------------------------------------------------------------------
+void Herkulex::rxPacket(uint8_t packetSize, uint8_t* data)
+{
+    #ifdef HERKULEX_DEBUG
+        pc->printf("[RX]");
+    #endif
+       
+    for (uint8_t i=0; i < packetSize; i++) 
+    {   
+        data[i] = rxd->getc();
+        
+        #ifdef HERKULEX_DEBUG
+            pc->printf("%02X ",data[i]);
+        #endif
+    }      
+    
+    #ifdef HERKULEX_DEBUG
+        pc->printf("\n");
+    #endif
+}
+
+//------------------------------------------------------------------------------
 void Herkulex::clear(uint8_t id)
 {
     uint8_t txBuf[11];
@@ -113,7 +134,7 @@
     txBuf[6] = 0;                   // Checksum2
     txBuf[7] = RAM_TORQUE_CONTROL;  // Address 52
     txBuf[8] = BYTE1;               // Length
-    txBuf[9] = cmdTorue;           // Torque ON
+    txBuf[9] = cmdTorue;            // Torque ON
 
     // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
     // Checksum2 = (~Checksum1)&0xFE
@@ -125,7 +146,7 @@
 }
 
 //------------------------------------------------------------------------------
-void Herkulex::movePos(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED)
+void Herkulex::positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED)
 {
     if (position > 1023) return;
     if (playtime > 255) return;
@@ -155,7 +176,7 @@
 }
 
 //------------------------------------------------------------------------------
-void Herkulex::turn(uint8_t id, int16_t speed, uint8_t setLED)
+void Herkulex::velocityControl(uint8_t id, int16_t speed, uint8_t setLED)
 {
     if (speed > 1023 || speed < -1023) return;
     
@@ -184,5 +205,116 @@
 }
 
 //------------------------------------------------------------------------------
+int8_t Herkulex::getStatus(uint8_t id) 
+{
+    uint8_t status;
+    uint8_t txBuf[7];
+    
+    txBuf[0] = HEADER;                  // Packet Header (0xFF)
+    txBuf[1] = HEADER;                  // Packet Header (0xFF)
+    txBuf[2] = MIN_PACKET_SIZE;         // Packet Size
+    txBuf[3] = id;                      // Servo ID
+    txBuf[4] = CMD_STAT;                // Status Error, Status Detail request
 
+    // Check Sum1 and Check Sum2
+    txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]) & 0xFE;
+    txBuf[6] = (~txBuf[5])&0xFE;
 
+    // send packet (mbed -> herkulex)
+    txPacket(7, txBuf);
+    
+    uint8_t rxBuf[9];
+    rxPacket(9, rxBuf);
+
+    // Checksum1
+    uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]) & 0xFE;    
+    if (chksum1 != rxBuf[5])
+    {
+        #ifdef HERKULEX_DEBUG
+            pc->printf("Checksum1 fault\n");
+        #endif
+        
+        return -1;
+    }
+    
+    // Checksum2
+    uint8_t chksum2 = (~rxBuf[5]&0xFE);
+    if (chksum2 != rxBuf[6])
+    {
+        #ifdef HERKULEX_DEBUG
+            pc->printf("Checksum2 fault\n");
+        #endif
+        
+        return -1;
+    }
+
+    status = rxBuf[7];  // Status Error
+  //status = rxBuf[8];  // Status Detail
+    
+    #ifdef HERKULEX_DEBUG
+        pc->printf("Status = %02X\n", status);
+    #endif
+    
+    return status;
+}
+
+//------------------------------------------------------------------------------
+int16_t Herkulex::getPos(uint8_t id) 
+{
+    uint16_t position = 0;
+    
+    uint8_t txBuf[9];
+    
+    txBuf[0] = HEADER;                  // Packet Header (0xFF)
+    txBuf[1] = HEADER;                  // Packet Header (0xFF)
+    txBuf[2] = MIN_PACKET_SIZE + 2;     // Packet Size
+    txBuf[3] = id;                      // Servo ID
+    txBuf[4] = CMD_RAM_READ;            // Status Error, Status Detail request
+    txBuf[5] = 0;                       // Checksum1
+    txBuf[6] = 0;                       // Checksum2    
+    txBuf[7] = RAM_CALIBRATED_POSITION; // Address 52
+    txBuf[8] = BYTE2;                   // Address 52 and 53      
+
+    // Check Sum1 and Check Sum2
+    txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
+    txBuf[6] = (~txBuf[5])&0xFE;
+
+    // send packet (mbed -> herkulex)
+    txPacket(9, txBuf);
+    
+    uint8_t rxBuf[13];
+    rxPacket(13, rxBuf);
+
+    // Checksum1
+    uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE;    
+    if (chksum1 != rxBuf[5])
+    {
+        #ifdef HERKULEX_DEBUG
+            pc->printf("Checksum1 fault\n");
+        #endif
+        
+        return -1;
+    }
+    
+    // Checksum2
+    uint8_t chksum2 = (~rxBuf[5]&0xFE);
+    if (chksum2 != rxBuf[6])
+    {
+        #ifdef HERKULEX_DEBUG
+            pc->printf("Checksum2 fault\n");
+        #endif
+        
+        return -1;
+    }
+
+    position = ((rxBuf[10]&0x03)<<8) | rxBuf[9];
+    
+    #ifdef HERKULEX_DEBUG
+        pc->printf("position = %04X(%d)\n", position, position);
+    #endif
+    
+    return position;
+}
+
+//------------------------------------------------------------------------------
+
--- a/herkulex.h	Mon Jan 14 16:24:05 2013 +0000
+++ b/herkulex.h	Mon Jan 14 21:03:43 2013 +0000
@@ -144,8 +144,7 @@
 
 //------------------------------------------------------------------------------
 // ACK Packet [To Controller(ACK)]
-#define CMD_ACK_MASK   0x40    // ACK Packet CMD is Request Packet CMD + 0x40
-
+#define CMD_ACK_MASK   0x40 // ACK Packet CMD is Request Packet CMD + 0x40
 #define CMD_EEP_WRITE_ACK   (CMD_EEP_WRITE|CMD_ACK_MASK)
 #define CMD_EEP_READ_ACK    (CMD_EEP_READ|CMD_ACK_MASK)
 #define CMD_RAM_WRITE_ACK   (CMD_RAM_WRITE|CMD_ACK_MASK)
@@ -157,6 +156,27 @@
 #define CMD_REBOOT_ACK      (CMD_REBOOT|CMD_ACK_MASK)
 
 //------------------------------------------------------------------------------
+// Status Error
+#define STATUS_OK                       = 0x00;
+#define ERROR_EXCEED_INPUT_VOLTAGE      = 0x01;
+#define ERROR_EXCEED_POT_LIMIT          = 0x02;
+#define ERROR_EXCEED_TEMPERATURE_LIMIT  = 0x04;
+#define ERROR_INVALID_PACKET            = 0x08;
+#define ERROR_OVERLOAD                  = 0x10;
+#define ERROR_DRIVER_FAULT              = 0x20;
+#define ERROR_EEP_REG_DISTORT           = 0x40;
+
+//------------------------------------------------------------------------------
+// Status Detail
+#define MOVING_FLAG                     = 0x01;
+#define INPOSITION_FLAG                 = 0x02;
+#define CHECKSUM_ERROR                  = 0x04; // Invalid packet`s detailed information
+#define UNKNOWN_COMMAND                 = 0x08; // Invalid packet`s detailed information
+#define EXCEED_REG_RANGE                = 0x10; // Invalid packet`s detailed information
+#define GARBAGE_DETECTED                = 0x20; // Invalid packet`s detailed information
+#define MOTOR_ON_FLAG                   = 0x40;
+
+//------------------------------------------------------------------------------
 // Header
 #define HEADER                              0xFF
 
@@ -245,6 +265,13 @@
      */  
     void txPacket(uint8_t packetSize, uint8_t* data);
     
+    /** Receive packet datas
+     *        
+     * @param packetSize The packet size.
+     * @param data The receive packet data array.
+     */ 
+    void rxPacket(uint8_t packetSize, uint8_t* data);
+    
     /** Clear error status
      *        
      * @param id The herkulex servo ID.
@@ -254,27 +281,40 @@
     /** Set torque setting
      *        
      * @param id The herkulex servo ID.
-     * @param cmdTorue The Command for setting of torque (TORQUE_FREE 0x00,BREAK_ON 0x40, TORQUE_ON 0x60)
+     * @param cmdTorue The Command for setting of torque (TORQUE_FREE 0x00, BREAK_ON 0x40, TORQUE_ON 0x60)
      */      
     void setTorque(uint8_t id, uint8_t cmdTorue);
     
-    /** move position
+    /** Position Control
      *        
      * @param id The herkulex servo ID.
      * @param position The goal position of herkulex servo.
      * @param playtime Time to target position.
-     * @param setLED Select LED and on/off controll (SET_LED_GREEN_ON 0x00,SET_LED_BLUE_ON 0x08, SET_LED_RED_ON 0x10)
+     * @param setLED Select LED and on/off controll (GLED_ON 0x00,BLED_ON 0x08, RLED_ON 0x10)
      */          
-    void movePos(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED);
+    void positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED);
 
-    /** turn
+    /** Velocity Control
      *        
      * @param id The herkulex servo ID.
      * @param speed The goal position of herkulex servo.
-     * @param setLED Select LED and on/off controll (SET_LED_GREEN_ON 0x00,SET_LED_BLUE_ON 0x08, SET_LED_RED_ON 0x10)
+     * @param setLED Select LED and on/off controll (GLED_ON 0x00,BLED_ON 0x08, RLED_ON 0x10)
      */             
-    void turn(uint8_t id, int16_t speed,uint8_t setLED);
+    void velocityControl(uint8_t id, int16_t speed,uint8_t setLED);
     
+    /** Get Status
+     *        
+     * @param id The herkulex servo ID.
+     * @return -1 is getStatus failed. other is servo`s status error value.
+     */       
+    int8_t getStatus(uint8_t id);
+    
+    /** Get Position
+     *        
+     * @param id The herkulex servo ID.
+     * @return -1 is getPos failed. other is servo's current position.
+     */         
+    int16_t getPos(uint8_t id);
          
 private :