Diff: herkulex.cpp

Revision:

5:f737e5c70115

Parent:

1:874d1f42989c

Child:

6:1dacff31b77a

--- a/herkulex.cpp	Mon Jan 14 07:25:00 2013 +0000
+++ b/herkulex.cpp	Mon Jan 14 16:24:05 2013 +0000
@@ -74,6 +74,32 @@
 }
 
 //------------------------------------------------------------------------------
+void Herkulex::clear(uint8_t id)
+{
+    uint8_t txBuf[11];
+    
+    txBuf[0] = HEADER;              // Packet Header (0xFF)
+    txBuf[1] = HEADER;              // Packet Header (0xFF)
+    txBuf[2] = MIN_PACKET_SIZE + 4; // Packet Size
+    txBuf[3] = id;                  // Servo ID
+    txBuf[4] = CMD_RAM_WRITE;       // Command Ram Write (0x03)
+    txBuf[5] = 0;                   // Checksum1
+    txBuf[6] = 0;                   // Checksum2
+    txBuf[7] = RAM_STATUS_ERROR;    // Address 48
+    txBuf[8] = BYTE2;               // Length
+    txBuf[9] = 0;                   // Clear RAM_STATUS_ERROR
+    txBuf[10]= 0;                   // Clear RAM_STATUS_DETAIL
+    
+    // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
+    // Checksum2 = (~Checksum1)&0xFE
+    txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]) & 0xFE;
+    txBuf[6] = (~txBuf[5])&0xFE;
+
+    // send packet (mbed -> herkulex)
+    txPacket(11, txBuf);
+}
+
+//------------------------------------------------------------------------------
 void Herkulex::setTorque(uint8_t id, uint8_t cmdTorue)
 {
     uint8_t txBuf[10];
@@ -99,9 +125,9 @@
 }
 
 //------------------------------------------------------------------------------
-void Herkulex::movePos(uint8_t id, uint16_t pos, uint8_t playtime, uint8_t setMode, uint8_t setLED)
+void Herkulex::movePos(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED)
 {
-    if (pos > 1023) return;
+    if (position > 1023) return;
     if (playtime > 255) return;
     
     uint8_t txBuf[12];
@@ -114,9 +140,38 @@
     txBuf[5]  = 0;                      // Checksum1
     txBuf[6]  = 0;                      // Checksum2
     txBuf[7]  = playtime;               // Playtime
-    txBuf[8]  = pos & 0x00FF;           // JOG(LSB)
-    txBuf[9]  =(pos & 0xFF00) >> 8;     // JOG(MSB)
-    txBuf[10] = setMode | setLED;       // Set mode and LED on/off
+    txBuf[8]  = position & 0x00FF;      // Position (LSB, Least Significant Bit)
+    txBuf[9]  =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
+    txBuf[10] = POS_MODE | setLED;      // Pos Mode and LED on/off
+    txBuf[11] = id;                     // Servo ID
+    
+    // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
+    // Checksum2 = (~Checksum1)&0xFE
+    txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
+    txBuf[6] = (~txBuf[5])&0xFE;
+
+    // send packet (mbed -> herkulex)
+    txPacket(12, txBuf);
+}
+
+//------------------------------------------------------------------------------
+void Herkulex::turn(uint8_t id, int16_t speed, uint8_t setLED)
+{
+    if (speed > 1023 || speed < -1023) return;
+    
+    uint8_t txBuf[12];
+    
+    txBuf[0]  = HEADER;                 // Packet Header (0xFF)
+    txBuf[1]  = HEADER;                 // Packet Header (0xFF)
+    txBuf[2]  = MIN_PACKET_SIZE + 5;    // Packet Size
+    txBuf[3]  = MAX_PID;                // pID is total number of servos in the network (0 ~ 253)
+    txBuf[4]  = CMD_S_JOG;              // Command S JOG (0x06)
+    txBuf[5]  = 0;                      // Checksum1
+    txBuf[6]  = 0;                      // Checksum2
+    txBuf[7]  = 0;                      // Playtime, unmeaningful in turn mode
+    txBuf[8]  = speed & 0x00FF;         // Speed (LSB, Least Significant Bit)
+    txBuf[9]  =(speed & 0xFF00) >> 8;   // Speed (MSB, Most Significanct Bit)
+    txBuf[10] = TURN_MODE | setLED;     // Turn Mode and LED on/off
     txBuf[11] = id;                     // Servo ID
     
     // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
@@ -130,3 +185,4 @@
 
 //------------------------------------------------------------------------------
 
+