Diff: bluetooth.cpp

Revision:

8:bfcfda6b38fe

Parent:

7:67720739ca77

Child:

9:6057314dc8ec

diff -r 67720739ca77 -r bfcfda6b38fe bluetooth.cpp
--- a/bluetooth.cpp	Fri Sep 12 23:01:17 2014 +0000
+++ b/bluetooth.cpp	Sat Sep 13 09:42:55 2014 +0000
@@ -1,17 +1,15 @@
 #include "mbed.h"
 #include "bluetooth.h"
 #include "control.h"
-#include "MODSERIAL.h"
+#include "RawSerial.h"
 
-//master
-//Serial bt(p13, p14);  // tx, rx
-//slave
-Serial bt(p28, p27);
+RawSerial bt(p28, p27);  // tx, rx
 
-Serial pc(USBTX, USBRX);   // tx, rx
+RawSerial pc(USBTX, USBRX);   // tx, rx
 
 DigitalOut l1(LED1);
 DigitalOut l2(LED2);
+DigitalOut l3(LED3);
 DigitalOut l4(LED4);
 
 void btSetup(int role)
@@ -36,7 +34,7 @@
     Cvt temp;
     
     //making pac
-    pac[0] = option;
+    pac[0]=option;
     if( option==SYNC_MOTOR ){
         //PACK:     [option/function/pwm*4]
         //function
@@ -73,11 +71,15 @@
     
     wait(1/1000.0);
     
-    l1=( l1 ? 0 : 1 );
+    //l1=( l1 ? 0 : 1 );
+    
+    l4=1;
     
     for( int i=0 ; i<PACK_SIZE ; i++ ){
         buf[i]=bt.getc();
-    }  
+    }
+    
+    l3=1;
  
     if( buf[0]==SYNC_MOTOR ){
         //PACK:     [option/function/pwm*4]
@@ -91,14 +93,14 @@
         //PACK:     [option/request]
         ;       //not yet
     }
-    else if( buf[0]==SYNC_SENSOR ){        
+    else if( buf[0]==SYNC_SENSOR ){
+        l2=1;    
         getSensor( &(val[0]), &(val[1]) );
         //PACK:     [option/ir*4/fsr*4];
         //option
         pac[0] = SYNC_SENSOR;
         //ir
         temp.fl = val[0];
-        temp.fl = ++i%2;
         for( int i=0 ; i<4 ; i++ ){
             pac[1+i] = temp.byte[i];    
         }
@@ -107,7 +109,7 @@
         for( int i=0 ; i<4 ; i++ ){
             pac[5+i] = temp.byte[i];    
         }
-        
+        pc.printf( "%f\n", val[1] );
         //send pac
         for( int i=0 ; i<PACK_SIZE ; i++ ){
             bt.putc( pac[i] );
@@ -132,31 +134,43 @@
     char buf[PACK_SIZE]={};
     Cvt temp;
     
-    wait(0.05);
+    sync(SYNC_SENSOR,NULL,NULL);
+    wait(0.0001);
     
     l1= l1 ? 0 : 1;
-    
-    for( int i=0 ; ; i++ ){
+           
+    //Read
+    for(int i=0 ;;){
         if( bt.readable() ){
+            buf[0]=bt.getc();
             pc.printf("readable\n");
-            wait(0.05);
+            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 if( i>5 ){
-            l4 = 0;
-            pc.printf("not readable\n");
-            return;
+        else{
+            i++;
+            if( i>5 ){
+                l4 = 0;
+                pc.printf("not readable\n");
+                return;
+            }
         }    
     }
     
-    l4 = 1;
-
-    //Read
+   
     for( int i=0 ; i<PACK_SIZE ; i++ ){
-        buf[i]=bt.getc();
+        pc.printf("%02x ",buf[i]);
     }
-    
-    return;
+    pc.printf("\n");
+    l4 = 1;
     
     //PACK:     [option/ir*4/fsr*4];
     //option
@@ -175,5 +189,7 @@
         temp.byte[i] = buf[5+i];    
     }
     *_fsr = temp.fl;
+    
+    pc.printf( "%f %f\n", *_ir, *_fsr );
         
 }
\ No newline at end of file