File content as of revision 4:aaaadb45cbd9:

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

//master
Serial bt(p13, p14);  // tx, rx
//slave
//Serial bt(p28, p27);

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

DigitalOut l1(LED1);
DigitalOut l2(LED2);
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 );
    }
}

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]={};
    float val[PACK_SIZE/4+1]={};
    Cvt temp;
    
    wait(1/1000.0);
    
    l1=( l1 ? 0 : 1 );
    
    for( int i=0 ; i<PACK_SIZE ; i++ ){
        buf[i]=bt.getc();
    }  
 
    if( buf[0]==SYNC_MOTOR ){
        //PACK:     [option/function/pwm*4]
        //pwm
        for( int i=0 ; i<4 ; i++ ){
            temp.byte[i]=buf[1+i];
        }
        motor( buf[1], temp.fl );
    }
    else if( buf[0]==SYNC_FM ){
        //PACK:     [option/request]
        ;       //not yet
    }
    else if( buf[0]==SYNC_SENSOR ){        
        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];    
        }
        //fsr
        temp.fl = val[1];
        for( int i=0 ; i<4 ; i++ ){
            pac[5+i] = temp.byte[i];    
        }
        
        //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 receiveSensor(float* _ir, float* _fsr)
{
    char buf[PACK_SIZE]={};
    Cvt temp;
    
    wait(0.05);
    
    l1= l1 ? 0 : 1;
    
    for( int i=0 ; ; i++ ){
        if( bt.readable() ){
            pc.printf("readable\n");
            break;
        }
        else if( i>5 ){
            l4 = 0;
            pc.printf("enable\n");
            return;
        }    
    }
    
    l4 = 1;

    //Read
    for( int i=0 ; i<PACK_SIZE ; i++ ){
        buf[i]=bt.getc();
    }
    
    return;
    
    //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;
        
}