Diff: Mx28.cpp

Revision:

2:2e32ee9f0e51

Parent:

1:c866ce96ceb3

Child:

4:ffe33f18ab21

--- a/Mx28.cpp	Wed Sep 10 13:06:35 2014 +0000
+++ b/Mx28.cpp	Thu Apr 28 14:02:54 2016 +0000
@@ -944,4 +944,81 @@
     }
 }
 
+unsigned int DynamixelClass::torqueMode(unsigned char ID, bool Status){  
+  
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = 0x04;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = RAM_TORQUE_CONTROL_MODE_ENABLE;
+    Instruction_Packet_Array[4] = Status;
+    Instruction_Packet_Array[5] = ~(ID + 0x04 + COMMAND_WRITE_DATA + RAM_TORQUE_CONTROL_MODE_ENABLE + Status);   
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
 
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }   
+}
+
+unsigned int DynamixelClass::torque(unsigned char ID,unsigned int Torque){ 
+  
+            
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = 0x05;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = RAM_GOAL_TORQUE_L;
+    Instruction_Packet_Array[4] = (char)(Torque);
+    Instruction_Packet_Array[5] = (char)((Torque & 0x0F00) >> 8);    
+    Instruction_Packet_Array[6] = ~(ID + 0x05 + COMMAND_WRITE_DATA + RAM_GOAL_TORQUE_L  + (char)(Torque) + (char)((Torque & 0x0F00) >> 8) );            
+            
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }           
+
+}
+
+unsigned int DynamixelClass::readRegister(unsigned char ID,unsigned char Register){ 
+        
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = 0x04;
+    Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+    Instruction_Packet_Array[3] = Register;
+    Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
+    Instruction_Packet_Array[5] = ~(ID + READ_TEMP_LENGTH  + COMMAND_READ_DATA + Register + READ_ONE_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[3];
+    }else{
+        return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+    }
+}
+
+