Xavier Bouvard / herkulex

herkulex servo control library (fixes velocityControl and positionControl)

Fork of herkulex by Yoonseok Pyo

Files at this revision

API Documentation at this revision

Revision 7:fb5f919f2700, committed 2015-05-31

Comitter:
XavierBouvard
Date:
Sun May 31 15:08:32 2015 +0000
Parent:
6:1dacff31b77a
Commit message:
fixes herkulex protocole issues

Changed in this revision

herkulex.cpp Show annotated file Show diff for this revision Revisions of this file 
diff -r 1dacff31b77a -r fb5f919f2700 herkulex.cpp
--- a/herkulex.cpp	Mon Jan 14 21:03:43 2013 +0000
+++ b/herkulex.cpp	Sun May 31 15:08:32 2015 +0000
@@ -156,7 +156,7 @@
     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[3]  = id;                // 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
@@ -185,13 +185,22 @@
     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[3]  = id;                // 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)
+    
+    int16_t temp;
+    if (speed < 0) {
+        temp = speed * -1;
+        temp |= 0x4000;
+    } else {
+        temp = speed;
+    }
+
+    txBuf[8]  = temp & 0x00FF;         // Speed (LSB, Least Significant Bit)
+    txBuf[9]  =(temp & 0xFF00) >> 8;   // Speed (MSB, Most Significanct Bit)
     txBuf[10] = TURN_MODE | setLED;     // Turn Mode and LED on/off
     txBuf[11] = id;                     // Servo ID