Revision 6:1dacff31b77a, committed 2013-01-14

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

--- 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 :