Diff: Mx28.cpp

Revision:

1:c866ce96ceb3

Parent:

0:45d73665e91f

Child:

2:2e32ee9f0e51

--- a/Mx28.cpp	Wed Sep 10 10:36:12 2014 +0000
+++ b/Mx28.cpp	Wed Sep 10 13:06:35 2014 +0000
@@ -19,6 +19,12 @@
  printf("\r\n"); 
 }
 
+void DynamixelClass::debugStatusframe(void) {
+ for (int i=0; i<10; i++)
+  printf("%x,",Status_Packet_Array[i]);
+ printf("\r\n"); 
+}
+
 
 void DynamixelClass::transmitInstructionPacket(void){                                   // Transmit instruction packet to Dynamixel
 
@@ -55,32 +61,36 @@
     Status_Packet_Array[1] = 0x00;
     Status_Packet_Array[2] = 0x00;                                                      
     Status_Packet_Array[3] = 0x00;
-    i=0; RxState=0; j=0; 
+    i=0; RxState=0; j=0; InBuff[0]=0;
     Timer timer;
     timer.start();
 
 
-while ((servoSerial->readable()) &&(RxState<=3)){                              // Wait for " header + header + frame length + error " RX data
-   InBuff[i]=servoSerial->getc();
-   if (InBuff[i]==0xff) i++;                                                            // When we have the first header we starts to inc data to inbuffer
+while (RxState<3){                              // Wait for " header + header + frame length + error " RX data
+ if (timer.read_ms() >= STATUS_PACKET_TIMEOUT){
+        return Status_Packet_Array[2] = 0x80;                                      // Return with Error if Serial data not received with in time limit
+
+ }
 
-   if (timer.read_ms() >= STATUS_PACKET_TIMEOUT){
-        return Status_Packet_Array[2] = 0x80;                                      // Return with Error if Serial data not received with in time limit
-   }
+ if (servoSerial->readable()) {
+   InBuff[i]=servoSerial->getc();
+   if (InBuff[0]==0xff) i++;                                                            // When we have the first header we starts to inc data to inbuffer
+
    
-   if ((i>=STATUS_FRAME_BUFFER) &&(RxState==0)) RxState++; //read header
+   if ((i>=(STATUS_FRAME_BUFFER-1)) &&(RxState==0)) RxState++; //read header
    
    switch (RxState) {
     case 1: {//Read header 
             if ((InBuff[j] == 0xFF) &&  (InBuff[j+1]== 0xFF)){                                    // checkes that we have got the buffer
+                    j=2;
                     Status_Packet_Array[0] = InBuff[j++];                                    // ID sent from Dynamixel
                     Status_Packet_Array[1] = InBuff[j++];                                    // Frame Length of status packet
                     Status_Packet_Array[2] = InBuff[j++];                                    // Error (char) 
-                    RxState++;
+                    RxState++; led2->write(0);
             }
     } break;
     case 2: {//Read data
-      if (i>=Status_Packet_Array[1] - 2) { //We have recieved all data
+      if (i>Status_Packet_Array[1]+3) { //We have recieved all data
             do{
                 Status_Packet_Array[3 + Counter] = InBuff[j++];
                 Counter++;              
@@ -91,7 +101,11 @@
         }    
       } break;      
    } //switch 
+   }
   }//while..
+  timer.stop();
+//  debugStatusframe();
+
   return 0x00;
 }
 
@@ -106,7 +120,7 @@
     servoSerial->baud(baud);
     servoSerialDir= new DigitalOut(D_Pin);     
     servoSerialDir->write(0);
-
+    led2=new DigitalOut(LED2); 
 
 }   
 
@@ -923,7 +937,7 @@
     transmitInstructionPacket();
     readStatusPacket();
     
-    if (Status_Packet_Array[2] == 0){                   // If there is no status packet error return value
+    if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
         return (Status_Packet_Array[3]);            // Return Lock value
     }else{
         return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value