Diff: DynamixelBus.cpp

Revision:

1:c5327c6d6239

Parent:

0:35c27ebd9e8e

Child:

2:536f775454c3

--- a/DynamixelBus.cpp	Thu Dec 10 21:43:35 2015 +0000
+++ b/DynamixelBus.cpp	Tue Dec 15 04:38:25 2015 +0000
@@ -9,7 +9,7 @@
 
 DynamixelBus::DynamixelBus( PinName txUartPin, PinName rxUartPin, PinName txSelectPin, PinName rxSelectPin, int baud )
     : _uart( txUartPin, rxUartPin), _txSelect(txSelectPin, 0), _rxSelect(rxSelectPin, 0) {
-    _responseTimeout = .1;
+    _responseTimeout = .004;
     _baud = baud;
     _uart.baud(_baud);
     _replyDelay = .0012;
@@ -92,7 +92,7 @@
 StatusCode DynamixelBus::send_ping( ServoId id )
 {
     // 0xff, 0xff, ID, Length, Instruction(ping), Checksum
-    CommBuffer txBuf(6);
+    CommBuffer txBuf;
 
     txBuf.push_back( 0xff );   
     txBuf.push_back( 0xff );
@@ -109,44 +109,49 @@
     }
 
     // Wait for data to transmit
-    wait( (double)txBuf.size() * 8.0 * 1.0/_baud * 2.0 );
+    double transmitDelay = (double)txBuf.size() * 8.0 * 1.0/(double)_baud;
+    wait( transmitDelay );
 
     // swap inputs for half duplex communication
     _txSelect = 0;
     _rxSelect = 1;
 
-    wait( _replyDelay );
+    // 0xff, 0xff, ID, Length, Error, Checksum
+    CommBuffer replyBuf;
 
-    // 0xff, 0xff, ID, Length, Error, Checksum
-    CommBuffer rxBuf(6);
-
-    // we'll only get a reply if it was not broadcast
+    // we'll only get a reply if it was not broadcast (TODO: and status reply option is selected in servo)
     bool statusReceived = false;
+    Timer t;
     if( id != 0xFE )
     {
-        Timer timeout;
-        timeout.start();
-        while( timeout.read() < _responseTimeout )                
+        t.start();
+
+        while( t.read() < _responseTimeout )
         {
-            if (_uart.readable())
+            if( _uart.readable())
             {
-                rxBuf.push_back( _uart.getc() );
-            }
+                // BUGBUG: unsigned char b = _uart.getc();
+                // Bug with _uart.getc() on FRDM-K64F. Instead read directly from uart
+                char b = UART3->D;
+                
+                replyBuf.push_back( b );
             
-            if( rxBuf.size() == 6)
-            {
-                statusReceived = true;
-                break;
+                if( replyBuf.size() == 6)
+                {
+                    statusReceived = true;
+                    break;
+                }
             }
         }
-        timeout.stop();
+        
+        t.stop();
     }
-    
+
     _rxSelect = 0;
 
     if( statusReceived )
     {
-        return (unsigned char)(rxBuf[4] & statusValid);
+        return (unsigned char)(replyBuf[4] | statusValid);
     }
     else
     {
@@ -170,42 +175,47 @@
     txBuf.push_back( bytesToRead );
     txBuf.push_back( CalculateTxPacketChecksum( txBuf ) );
 
+    _txSelect = 1;
+
     for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
     {
         _uart.putc(*itSend);
     }
 
     // Wait for data to transmit
-    wait( (double)txBuf.size() * 8.0 * 1.0/_baud * 2.0 );
+    double transmitDelay = (double)txBuf.size() * 8.0 * 1.0/(double)_baud;
+    wait( transmitDelay );
+
     _txSelect = 0;
     _rxSelect = 1;
 
-    wait( _replyDelay );
-
     // 0xff, 0xff, ID, Length, Error, Data, Checksum
     int replySize = 6 + bytesToRead;
-    CommBuffer replyBuf( replySize );
+    CommBuffer replyBuf;
 
     // we'll only get a reply if it was not broadcast (and status reply option is selected in servo)
     bool statusReceived = false;
+    Timer t;
     if( id != 0xFE )
     {
-        Timer timeout;
-        timeout.start();
-        while( timeout.read() < _responseTimeout )                
+        t.start();
+        while( t.read() < _responseTimeout )                
         {
             if (_uart.readable())
             {
-                replyBuf.push_back( _uart.getc() );
-            }
-            
-            if( replyBuf.size() == replySize )
-            {
-                statusReceived = true;
-                break;
+                // BUGBUG: unsigned char b = _uart.getc();
+                // Bug with _uart.getc() on FRDM-K64F. Instead read directly from uart
+                char b = UART3->D;
+                replyBuf.push_back( b );
+
+                if( replyBuf.size() == replySize )
+                {
+                    statusReceived = true;
+                    break;
+                }
             }
         }
-        timeout.stop();
+        t.stop();
     }
     
     _rxSelect = 0;
@@ -216,7 +226,7 @@
         {
             data.push_back( replyBuf[5 + n] );
         }
-        return replyBuf[4] & statusValid;
+        return replyBuf[4] | statusValid;
     }
     else
     {
@@ -244,13 +254,6 @@
     }
     txBuf.push_back( CalculateTxPacketChecksum( txBuf ) );
 
-//    printf( "sending: " );
-    for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
-    {
-//        printf( "%02x ", *itSend);
-    }
-//    printf("\n");
-
     // enable transmission
     _txSelect = 1;
 
@@ -277,14 +280,13 @@
     {
         t.start();
 
-        while( t.read() < .002f )
+        while( t.read() < _responseTimeout )
         {
             if( _uart.readable())
             {
-//                unsigned char b = _uart.getc();
-// BUG with _uart.getc() on FRDM-K64F. Instead read directly from uart
+                // BUGBUG: unsigned char b = _uart.getc();
+                // Bug with _uart.getc() on FRDM-K64F. Instead read directly from uart
                 char b = UART3->D;
-                
                 replyBuf.push_back( b );
             
                 if( replyBuf.size() == 6)
@@ -300,19 +302,9 @@
 
     _rxSelect = 0;
 
-//    printf( "reply: ");
-
-    for( CommBuffer::iterator itRepl = replyBuf.begin(); itRepl != replyBuf.end(); itRepl++)
-    {
-//        printf( "%02x ", *itRepl);
-    }
-
-//    printf("\n");
-  
-
     if( statusReceived )
     {
-        return replyBuf[4] & statusValid;
+        return replyBuf[4] | statusValid;
     }
     else
     {