Diff: JrkG2.cpp

Revision:

2:e78c0ddcf337

Parent:

0:33bfb28b0ffc

--- a/JrkG2.cpp	Sat Aug 25 13:15:38 2018 +0000
+++ b/JrkG2.cpp	Sun Aug 26 08:42:11 2018 +0000
@@ -1,5 +1,7 @@
 #include <JrkG2.h>
 
+#define serial_delay_us 1500    //baud:9600
+
 /**** JrkG2Serial ****/
 
 void JrkG2Serial::commandW7(uint8_t cmd, uint8_t val)
@@ -25,8 +27,10 @@
     uint8_t val;
     
     sendCommandHeader(cmd);
+    wait_us(serial_delay_us);
     if(_stream->readable() != 1)
     {
+        _stream->abort_read();
         _lastError = JrkG2CommReadError;
         return 0;
     }
@@ -41,12 +45,10 @@
     
     sendCommandHeader(cmd);
     
-    //if(_stream->ByteCount() != 2)  //バッファリング未実装のため不可 
-    // → BufferedSerialを使用すべきか, タイムアウトも実装したい
-    
-    //これ以降も同様の理由で修正される
-    if(_stream->readable() == 0)
+    wait_us(serial_delay_us);
+    if(_stream->readable() != 2)
     {
+        _stream->abort_read();
         _lastError = JrkG2CommReadError;
         return 0;
     }
@@ -65,11 +67,12 @@
     
     sendCommandHeader(cmd);
     serialW7(offset);
+    wait_us(serial_delay_us);
     serialW7(length);
-    
-    //if(_stream->ByteCount() != length)
-    if(_stream->readable() != 1)
+    wait_us(serial_delay_us);
+    if(_stream->readable() != length)
     {
+        _stream->abort_read();
         _lastError = JrkG2CommReadError;
         // Set the buffer bytes to 0 so the program will not use an uninitialized
         // variable.