Diff: Mx28.cpp

Revision:

4:ffe33f18ab21

Parent:

2:2e32ee9f0e51

--- a/Mx28.cpp	Mon Jul 17 02:19:12 2017 +0000
+++ b/Mx28.cpp	Tue Oct 24 02:36:58 2017 +0000
@@ -43,7 +43,8 @@
     }while((Instruction_Packet_Array[1] - 2) >= Counter);
     
     servoSerial->putc(Instruction_Packet_Array[Counter + 2]);                               // Write check sum to serial
-    wait_us((Counter + 4)*190);
+    //wait_us((Counter + 4)*190);
+    wait_us((Counter + 4)*11);
     servoSerialDir->write(0);                                                          // Set TX Buffer pin to LOW after data has been sent
 
 }
@@ -878,6 +879,30 @@
     }
 }
 
+
+unsigned int DynamixelClass::readCurrent(unsigned char ID){   
+    
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = READ_CURRENT_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+    Instruction_Packet_Array[3] = RAM_PRESENT_CURRENT_L;
+    Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH;
+    Instruction_Packet_Array[5] = ~(ID + READ_CURRENT_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_CURRENT_L + READ_TWO_BYTE_LENGTH);
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();
+    readStatusPacket();
+    
+    if (Status_Packet_Array[2] == 0){                                           // If there is no status packet error return value
+        return (Status_Packet_Array[4] << 8) | Status_Packet_Array[3];  // Return present position value
+    }else{
+        return (Status_Packet_Array[2] | 0xF000);                           // If there is a error Returns error value
+    }
+}
+
+
 unsigned int DynamixelClass::checkRegister(unsigned char ID){    
         
     Instruction_Packet_Array[0] = ID;