Diff: MX12.cpp

Revision:

2:02f3323a107d

Parent:

0:7556356a8bcd

Child:

3:add8b050eb86

--- a/MX12.cpp	Wed Nov 03 12:56:02 2021 +0000
+++ b/MX12.cpp	Thu Nov 04 06:12:21 2021 +0000
@@ -56,33 +56,80 @@
     printf("\r\n");
 }
 
-void MX12::rw(unsigned char mot_id, char adress, char len, char *data) {
+void MX12::rw(unsigned char mot_id, char address, char len, char *data) {
     _res_count = 0;
     memset(_res, 0, MX12_ANSWER_MAX_SIZE);
     
-    // Forge packet
+    /* Forge instruction packet */
     char cmd[16];
     
+    /* Instruction Packet is the command data sent to the Device.
+     *
+     * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum |
+     * |-------|-------|---------|------|-----------|---------------|---------|
+     * |  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])
+     */
+    
+    /* header 1 = 0xFF (dynamixel protocol 1.0) */
     cmd[0] = 0xff;
-    cmd[1] = 0xff;
-    cmd[4] = 0x02 + (data != NULL);
     
+    /* header 2 = 0xFF (dynamixel protocol 1.0) */
+    cmd[1] = 0xff; 
+    
+     /* packet ID i.e. servomotor id (dynamixel protocol 1.0) */
     cmd[2] = mot_id;
+
+    /* Length indicates the byte size of the instruction, 
+       parameter and Checksum field */
+       
     
-    if(data == NULL) cmd[3] = 4;
-    else cmd[3] = 3 + len;
+    /* 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; // 
+
+        /* set write instruction */
+        cmd[4] = PROTOCOL_INSTRUCTION_READ;
+        
+        /* Param 1: address to read in the "Control Table of RAM Area" */
+        cmd[5] = address;
+    }
+    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 */
+        cmd[3] = 3 + len; 
+
+        /* set read instruction */
+        cmd[4] = PROTOCOL_INSTRUCTION_WRITE;
+
+        /* Param 1: address to write in the "Control Table of RAM Area" */
+        cmd[5] = address;
+    }
     
-    cmd[5] = adress;
+    //cmd[4] = 0x02 + (data != NULL); // original code from Titouan
+    
     
     // Compute checksum
     if(data == NULL) {
         cmd[6] = len;
-        cmd[7] = 0xFF - (mot_id + 4 + 2 + adress + len);
+        cmd[7] = 0xFF - (mot_id + 4 + 2 + address + len);
         
         // [XXX] Force length to one to force send
         len = 1;
     } else {
-        char cs = mot_id + len + adress + 6;
+        char cs = mot_id + len + address + 6;
         
         for(char i = 0; i < len; i++) {
             cmd[6 + i] = data[i];