Diff: MX12.cpp

Revision:

3:add8b050eb86

Parent:

2:02f3323a107d

Child:

5:0cf54586a4be

--- a/MX12.cpp	Thu Nov 04 06:12:21 2021 +0000
+++ b/MX12.cpp	Thu Nov 04 07:21:32 2021 +0000
@@ -62,6 +62,7 @@
     
     /* Forge instruction packet */
     char cmd[16];
+    char checksum = 0x00;
     
     /* Instruction Packet is the command data sent to the Device.
      *
@@ -69,12 +70,13 @@
      * |-------|-------|---------|------|-----------|---------------|---------|
      * |  0xFF |  0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM  |
      * | cmd[0]| cmd[1]|  cmd[2] |cmd[3]|  cmd[4]   |cmd[5]...      |(2 bytes)|
-     *                                                     \__  ___/
-     *                                              |         \/              |
-     *                                              |        len = N-1        |
-     *                                              \____________  ___________/
-     *                                                           \/
-     *                                                         Length (cmd[4])
+     *                                               \_  _/ \__  __/
+     *                                  |              \/      \/             |
+     *                                  |           address   data            |
+     *                                  |                     (len = N-1)     |
+     *                                  \__________________  _________________/
+     *                                                     \/
+     *                                                  Length ( cmd[3] )
      */
     
     /* header 1 = 0xFF (dynamixel protocol 1.0) */
@@ -85,6 +87,7 @@
     
      /* packet ID i.e. servomotor id (dynamixel protocol 1.0) */
     cmd[2] = mot_id;
+    checksum += cmd[2];
 
     /* Length indicates the byte size of the instruction, 
        parameter and Checksum field */
@@ -93,29 +96,45 @@
     /* Guess instruction type. NULL for read, not NULL for write */
     if(data == NULL) // read instruction
     {
-        /* byte length of the instruction: parameter and checksum field. */
-        /* for read instruction:                                         */
-        /*         2 PARAM (starting address, length of data) + 2 CHKSUM */
-        cmd[3] = 4; // 
+        /* byte length of the instruction: parameter and checksum field.   */
+        /* for read instruction:   1 INSTR +                               */
+        /*       2 PARAM (starting address, length of data) + 1 CHKSUM     */
+        cmd[3] = 4;
+        checksum += cmd[3];
 
         /* set write instruction */
         cmd[4] = PROTOCOL_INSTRUCTION_READ;
+        checksum += cmd[4];
         
-        /* Param 1: address to read in the "Control Table of RAM Area" */
+        /* Param 1: address to read in the Control Table of RAM Area */
         cmd[5] = address;
+        checksum += cmd[5];
+        
+        /* Param 2: number of bytes to read in the Control Table of RAM Area */
+        cmd[6] = len;
+        checksum += cmd[6];
+        
+        /* Checksum */
+        /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */
+        cmd[7] = ~checksum;
+        cmd[7] = 0xFF - (mot_id + 4 + 2 + address + len);
+
     }
     else // write instruction
     {
         /* byte length of the instruction: parameter and checksum field     */
-        /* For write instruction:                                           */
-        /*    (1 + len) PARAM (starting Address, bytes to write) + 2 CHKSUM */
+        /* For write instruction:   1 INSTR +                               */
+        /*       (1+len)PARAM (starting Address, bytes to write) + 1 CHKSUM */
         cmd[3] = 3 + len; 
+        checksum += cmd[3];
 
         /* set read instruction */
         cmd[4] = PROTOCOL_INSTRUCTION_WRITE;
+        checksum += cmd[4];
 
         /* Param 1: address to write in the "Control Table of RAM Area" */
         cmd[5] = address;
+        checksum += cmd[5];
     }
     
     //cmd[4] = 0x02 + (data != NULL); // original code from Titouan