Diff: Mx28.cpp

Revision:

5:ca1e55903d09

Parent:

4:e00500b45c78

diff -r e00500b45c78 -r ca1e55903d09 Mx28.cpp
--- a/Mx28.cpp	Tue Feb 14 07:01:19 2017 +0000
+++ b/Mx28.cpp	Mon Jul 23 08:35:17 2018 +0000
@@ -357,6 +357,33 @@
 
 }
 
+unsigned int DynamixelClass::setAlarmLED(unsigned char  ID,unsigned char Set){
+
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = SET_ALARM_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = EEPROM_ALARM_LED;
+    Instruction_Packet_Array[4] = Set;
+    Instruction_Packet_Array[5] = ~(ID + SET_ALARM_LENGTH + COMMAND_WRITE_DATA + EEPROM_ALARM_LED + Set);  
+    
+    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::setStatusPaket(unsigned char  ID,unsigned char Set){
 
     Instruction_Packet_Array[0] = ID;