File content as of revision 10:a90935ea0a4b:

#include "mbed.h"
#include "bluetooth.h"
#include "control.h"
#include "RawSerial.h"

RawSerial bt(p28, p27);  // tx, rx

RawSerial pc(USBTX, USBRX);   // tx, rx

DigitalOut l1(LED1);
DigitalOut l2(LED2);
DigitalOut l3(LED3);
DigitalOut l4(LED4);

void btSetup(int role)
{
    if( role==BT_MASTER ){
    //if this device is the master
        bt.baud(9600);
        bt.printf("$$$");
        wait(0.5);
        bt.printf("C\r");
        wait(0.5);
    }
    else{
    //if this device is the slave
        bt.attach( slaveRecieve, Serial::RxIrq );
    }
}

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]={};
    Cvt temp;
    
    //making pac
    pac[0]=option;
    if( option==SYNC_MOTOR ){
        //PACK:     [option/function/pwm*4]
        //function
        pac[1]=b_data[0];
        //pwm
        temp.fl = f_data[0];
        for( int i=0 ; i<4 ; i++ ){
            pac[2+i] = temp.byte[i];    
        }
    }
    else if( option==SYNC_FM ){
        //PACK:     [option/request]
        pac[1]=b_data[0];   //request
    }
    else{
        ;
    }
    
    //send pac
    for( int i=0 ; i<PACK_SIZE ; i++ ){
        bt.putc( pac[i] );
        pc.printf("%02x ",pac[i]);
    }
    pc.printf("\n");
}

void slaveRecieve(void)
{
    //static int i=0;
    char buf[PACK_SIZE]={};
    char pac[PACK_SIZE]={};
    int read_error;
    float val[PACK_SIZE/4+1]={};
    Cvt temp;
    
    wait(1/1000.0);
    
    //l1=( l1 ? 0 : 1 );
    
    l4=1;
    
    //read_error = readPack( buf );
    //if( read_error ) return;
    
    for( int i=0 ; i<PACK_SIZE ; i++ ){
        buf[i]=bt.getc();
    }
    
    l3=1;
 
    if( buf[0]==SYNC_MOTOR ){
        //PACK:     [option/function/pwm*4]
        //pwm
        for( int i=0 ; i<4 ; i++ ){
            temp.byte[i]=buf[2+i];
        }
        motor( buf[1], temp.fl );
    }
    else if( buf[0]==SYNC_FM ){
        //PACK:     [option/request]
        ;       //not yet
    }
    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];
        for( int i=0 ; i<4 ; i++ ){
            pac[1+i] = temp.byte[i];    
        }
        //fsr
        temp.fl = val[1];
        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] );
            pc.printf("%02x ",pac[i]);
        }
        pc.printf("\n");
        
        l2 = l2 ? 0 : 1;
    }
    else if( buf[0]==SYNC_FM ){
        ;       //not yet
        
        //send pac
        for( int i=0 ; i<PACK_SIZE ; i++ ){
            bt.putc( pac[i] );
        }
    }
}

void recieveSensor(float* _ir, float* _fsr)
{
    char buf[PACK_SIZE]={};
    int read_error;
    Cvt temp;
    
    sync(SYNC_SENSOR,NULL,NULL);
    wait(0.0001);
    
    l1= l1 ? 0 : 1;
           
    //Read
    read_error=readPack( buf );
    if( read_error )return;
   
    for( int i=0 ; i<PACK_SIZE ; i++ ){
        pc.printf("%02x ",buf[i]);
    }
    pc.printf("\n");
    l4 = 1;
    
    //PACK:     [option/ir*4/fsr*4];
    //option
    if( buf[0]!=SYNC_SENSOR ){
        return;
    }
    
    //ir
    for( int i=0 ; i<4 ; i++ ){
        temp.byte[i]=buf[1+i];    
    }
    *_ir = temp.fl;
    
    //fsr
    for( int i=0 ; i<4 ; i++ ){
        temp.byte[i] = buf[5+i];    
    }
    *_fsr = temp.fl;
    
    pc.printf( "%f %f\n", *_ir, *_fsr );
        
}

void sendMotor( char function, float power ){
    char f[1];
    float p[1];
    f[0] = function;
    p[0] = power;
    sync( SYNC_MOTOR, f, p );
}