Diff: bluetooth.cpp

Revision:

10:a90935ea0a4b

Parent:

9:6057314dc8ec

--- a/bluetooth.cpp	Sat Sep 13 11:03:52 2014 +0000
+++ b/bluetooth.cpp	Sun Sep 14 01:38:32 2014 +0000
@@ -28,6 +28,36 @@
     }
 }
 
+int readPack( char* buf ){
+    int error=0;
+    for(int i=0 ;;){
+        if( bt.readable() ){
+            buf[0]=bt.getc();
+            pc.printf("readable\n");
+            for( int j=1 ; j<PACK_SIZE ;){
+                if(bt.readable()){
+                    buf[j]=bt.getc();
+                    j++;
+                }else{
+                    pc.printf("hoge:%d\n",j); 
+                    error = j;  
+                    break; 
+                }
+            }
+            break;
+        }
+        else{
+            i++;
+            if( i>5 ){
+                l4 = 0;
+                pc.printf("not readable\n");
+                return -1;
+            }
+        }    
+    }
+    return error;
+}
+
 void sync(char option, char* b_data, float* f_data)
 {
     char pac[PACK_SIZE]={};
@@ -63,9 +93,10 @@
 
 void slaveRecieve(void)
 {
-    static int i=0;
+    //static int i=0;
     char buf[PACK_SIZE]={};
     char pac[PACK_SIZE]={};
+    int read_error;
     float val[PACK_SIZE/4+1]={};
     Cvt temp;
     
@@ -75,6 +106,9 @@
     
     l4=1;
     
+    //read_error = readPack( buf );
+    //if( read_error ) return;
+    
     for( int i=0 ; i<PACK_SIZE ; i++ ){
         buf[i]=bt.getc();
     }
@@ -132,6 +166,7 @@
 void recieveSensor(float* _ir, float* _fsr)
 {
     char buf[PACK_SIZE]={};
+    int read_error;
     Cvt temp;
     
     sync(SYNC_SENSOR,NULL,NULL);
@@ -140,31 +175,8 @@
     l1= l1 ? 0 : 1;
            
     //Read
-    for(int i=0 ;;){
-        if( bt.readable() ){
-            buf[0]=bt.getc();
-            pc.printf("readable\n");
-            for( int j=1 ; j<PACK_SIZE ;){
-                if(bt.readable()){
-                    buf[j]=bt.getc();
-                    j++;
-                }else{
-                    pc.printf("hoge:%d\n",j);   
-                    break; 
-                }
-            }
-            break;
-        }
-        else{
-            i++;
-            if( i>5 ){
-                l4 = 0;
-                pc.printf("not readable\n");
-                return;
-            }
-        }    
-    }
-    
+    read_error=readPack( buf );
+    if( read_error )return;
    
     for( int i=0 ; i<PACK_SIZE ; i++ ){
         pc.printf("%02x ",buf[i]);