Ohnishi_Gundan / Mbed 2 deprecated Master-FM

FM-test

Dependencies:   MODSERIAL mbed-rtos mbed

Fork of Master by Ohnishi_Gundan

bluetooth.cpp@5:37733f175430, 2014-09-12 (annotated)

Committer:
9uS7
Date:
Fri Sep 12 11:43:36 2014 +0000
Revision:
5:37733f175430
Parent:
4:aaaadb45cbd9
Child:
6:df6d8ba1907a
09/12/14

Who changed what in which revision?

UserRevisionLine numberNew contents of line
9uS7 0:4f07ba929908 1 #include "mbed.h"
9uS7 0:4f07ba929908 2 #include "bluetooth.h"
9uS7 2:c610e1a7fbcd 3 #include "control.h"
9uS7 5:37733f175430 4 #include "MODSERIAL.h"
9uS7 0:4f07ba929908 5
9uS7 3:12e1f116ea42 6 //master
9uS7 5:37733f175430 7 //RawSerial bt(p13, p14); // tx, rx
9uS7 3:12e1f116ea42 8 //slave
9uS7 5:37733f175430 9 MODSERIAL bt(p28, p27);
9uS7 3:12e1f116ea42 10
9uS7 5:37733f175430 11 MODSERIAL pc(USBTX, USBRX); // tx, rx
9uS7 4:aaaadb45cbd9 12
9uS7 3:12e1f116ea42 13 DigitalOut l1(LED1);
9uS7 4:aaaadb45cbd9 14 DigitalOut l2(LED2);
9uS7 4:aaaadb45cbd9 15 DigitalOut l4(LED4);
9uS7 0:4f07ba929908 16
9uS7 1:e1cfb5850088 17 void btSetup(int role)
9uS7 0:4f07ba929908 18 {
9uS7 1:e1cfb5850088 19 if( role==BT_MASTER ){
9uS7 1:e1cfb5850088 20 //if this device is the master
9uS7 1:e1cfb5850088 21 bt.baud(9600);
9uS7 1:e1cfb5850088 22 bt.printf("$$$");
9uS7 1:e1cfb5850088 23 wait(0.5);
9uS7 1:e1cfb5850088 24 bt.printf("C\r");
9uS7 1:e1cfb5850088 25 wait(0.5);
9uS7 1:e1cfb5850088 26 }
9uS7 1:e1cfb5850088 27 else{
9uS7 1:e1cfb5850088 28 //if this device is the slave
9uS7 3:12e1f116ea42 29 bt.attach( slaveRecieve, Serial::RxIrq );
9uS7 3:12e1f116ea42 30 }
9uS7 2:c610e1a7fbcd 31 }
9uS7 3:12e1f116ea42 32
9uS7 3:12e1f116ea42 33 void sync(char option, char* b_data, float* f_data)
9uS7 2:c610e1a7fbcd 34 {
9uS7 4:aaaadb45cbd9 35 char pac[PACK_SIZE]={};
9uS7 3:12e1f116ea42 36 Cvt temp;
9uS7 3:12e1f116ea42 37
9uS7 3:12e1f116ea42 38 //making pac
9uS7 3:12e1f116ea42 39 pac[0] = option;
9uS7 3:12e1f116ea42 40 if( option==SYNC_MOTOR ){
9uS7 3:12e1f116ea42 41 //PACK: [option/function/pwm*4]
9uS7 3:12e1f116ea42 42 //function
9uS7 3:12e1f116ea42 43 pac[1]=b_data[0];
9uS7 3:12e1f116ea42 44 //pwm
9uS7 3:12e1f116ea42 45 temp.fl = f_data[0];
9uS7 3:12e1f116ea42 46 for( int i=0 ; i<4 ; i++ ){
9uS7 3:12e1f116ea42 47 pac[2+i] = temp.byte[i];
9uS7 3:12e1f116ea42 48 }
9uS7 3:12e1f116ea42 49 }
9uS7 3:12e1f116ea42 50 else if( option==SYNC_FM ){
9uS7 3:12e1f116ea42 51 //PACK: [option/request]
9uS7 3:12e1f116ea42 52 pac[1]=b_data[0]; //request
9uS7 1:e1cfb5850088 53 }
9uS7 2:c610e1a7fbcd 54 else{
9uS7 3:12e1f116ea42 55 ;
9uS7 3:12e1f116ea42 56 }
9uS7 3:12e1f116ea42 57
9uS7 3:12e1f116ea42 58 //send pac
9uS7 3:12e1f116ea42 59 for( int i=0 ; i<PACK_SIZE ; i++ ){
9uS7 4:aaaadb45cbd9 60 bt.putc( pac[i] );
9uS7 4:aaaadb45cbd9 61 pc.printf("%02x ",pac[i]);
9uS7 2:c610e1a7fbcd 62 }
9uS7 4:aaaadb45cbd9 63 pc.printf("\n");
9uS7 2:c610e1a7fbcd 64 }
9uS7 3:12e1f116ea42 65
9uS7 3:12e1f116ea42 66 void slaveRecieve(void)
9uS7 3:12e1f116ea42 67 {
9uS7 4:aaaadb45cbd9 68 static int i=0;
9uS7 4:aaaadb45cbd9 69 char buf[PACK_SIZE]={};
9uS7 4:aaaadb45cbd9 70 char pac[PACK_SIZE]={};
9uS7 4:aaaadb45cbd9 71 float val[PACK_SIZE/4+1]={};
9uS7 3:12e1f116ea42 72 Cvt temp;
9uS7 3:12e1f116ea42 73
9uS7 3:12e1f116ea42 74 wait(1/1000.0);
9uS7 3:12e1f116ea42 75
9uS7 4:aaaadb45cbd9 76 l1=( l1 ? 0 : 1 );
9uS7 3:12e1f116ea42 77
9uS7 3:12e1f116ea42 78 for( int i=0 ; i<PACK_SIZE ; i++ ){
9uS7 3:12e1f116ea42 79 buf[i]=bt.getc();
9uS7 3:12e1f116ea42 80 }
9uS7 3:12e1f116ea42 81
9uS7 3:12e1f116ea42 82 if( buf[0]==SYNC_MOTOR ){
9uS7 3:12e1f116ea42 83 //PACK: [option/function/pwm*4]
9uS7 3:12e1f116ea42 84 //pwm
9uS7 3:12e1f116ea42 85 for( int i=0 ; i<4 ; i++ ){
9uS7 3:12e1f116ea42 86 temp.byte[i]=buf[1+i];
9uS7 3:12e1f116ea42 87 }
9uS7 3:12e1f116ea42 88 motor( buf[1], temp.fl );
9uS7 3:12e1f116ea42 89 }
9uS7 3:12e1f116ea42 90 else if( buf[0]==SYNC_FM ){
9uS7 3:12e1f116ea42 91 //PACK: [option/request]
9uS7 3:12e1f116ea42 92 ; //not yet
9uS7 3:12e1f116ea42 93 }
9uS7 3:12e1f116ea42 94 else if( buf[0]==SYNC_SENSOR ){
9uS7 3:12e1f116ea42 95 getSensor( &(val[0]), &(val[1]) );
9uS7 3:12e1f116ea42 96 //PACK: [option/ir*4/fsr*4];
9uS7 3:12e1f116ea42 97 //option
9uS7 3:12e1f116ea42 98 pac[0] = SYNC_SENSOR;
9uS7 3:12e1f116ea42 99 //ir
9uS7 3:12e1f116ea42 100 temp.fl = val[0];
9uS7 3:12e1f116ea42 101 temp.fl = ++i%2;
9uS7 3:12e1f116ea42 102 for( int i=0 ; i<4 ; i++ ){
9uS7 3:12e1f116ea42 103 pac[1+i] = temp.byte[i];
9uS7 3:12e1f116ea42 104 }
9uS7 3:12e1f116ea42 105 //fsr
9uS7 3:12e1f116ea42 106 temp.fl = val[1];
9uS7 3:12e1f116ea42 107 for( int i=0 ; i<4 ; i++ ){
9uS7 3:12e1f116ea42 108 pac[5+i] = temp.byte[i];
9uS7 3:12e1f116ea42 109 }
9uS7 3:12e1f116ea42 110
9uS7 3:12e1f116ea42 111 //send pac
9uS7 3:12e1f116ea42 112 for( int i=0 ; i<PACK_SIZE ; i++ ){
9uS7 3:12e1f116ea42 113 bt.putc( pac[i] );
9uS7 4:aaaadb45cbd9 114 pc.printf("%02x ",pac[i]);
9uS7 3:12e1f116ea42 115 }
9uS7 4:aaaadb45cbd9 116 pc.printf("\n");
9uS7 4:aaaadb45cbd9 117
9uS7 4:aaaadb45cbd9 118 l2 = l2 ? 0 : 1;
9uS7 3:12e1f116ea42 119 }
9uS7 3:12e1f116ea42 120 else if( buf[0]==SYNC_FM ){
9uS7 3:12e1f116ea42 121 ; //not yet
9uS7 3:12e1f116ea42 122
9uS7 3:12e1f116ea42 123 //send pac
9uS7 3:12e1f116ea42 124 for( int i=0 ; i<PACK_SIZE ; i++ ){
9uS7 3:12e1f116ea42 125 bt.putc( pac[i] );
9uS7 3:12e1f116ea42 126 }
9uS7 3:12e1f116ea42 127 }
9uS7 3:12e1f116ea42 128 }
9uS7 3:12e1f116ea42 129
9uS7 3:12e1f116ea42 130 void receiveSensor(float* _ir, float* _fsr)
9uS7 3:12e1f116ea42 131 {
9uS7 4:aaaadb45cbd9 132 char buf[PACK_SIZE]={};
9uS7 3:12e1f116ea42 133 Cvt temp;
9uS7 3:12e1f116ea42 134
9uS7 4:aaaadb45cbd9 135 wait(0.05);
9uS7 4:aaaadb45cbd9 136
9uS7 4:aaaadb45cbd9 137 l1= l1 ? 0 : 1;
9uS7 3:12e1f116ea42 138
9uS7 4:aaaadb45cbd9 139 for( int i=0 ; ; i++ ){
9uS7 4:aaaadb45cbd9 140 if( bt.readable() ){
9uS7 4:aaaadb45cbd9 141 pc.printf("readable\n");
9uS7 5:37733f175430 142 wait(0.05);
9uS7 4:aaaadb45cbd9 143 break;
9uS7 4:aaaadb45cbd9 144 }
9uS7 4:aaaadb45cbd9 145 else if( i>5 ){
9uS7 4:aaaadb45cbd9 146 l4 = 0;
9uS7 5:37733f175430 147 pc.printf("not readable\n");
9uS7 4:aaaadb45cbd9 148 return;
9uS7 4:aaaadb45cbd9 149 }
9uS7 4:aaaadb45cbd9 150 }
9uS7 4:aaaadb45cbd9 151
9uS7 4:aaaadb45cbd9 152 l4 = 1;
9uS7 4:aaaadb45cbd9 153
9uS7 3:12e1f116ea42 154 //Read
9uS7 3:12e1f116ea42 155 for( int i=0 ; i<PACK_SIZE ; i++ ){
9uS7 3:12e1f116ea42 156 buf[i]=bt.getc();
9uS7 3:12e1f116ea42 157 }
9uS7 3:12e1f116ea42 158
9uS7 4:aaaadb45cbd9 159 return;
9uS7 4:aaaadb45cbd9 160
9uS7 3:12e1f116ea42 161 //PACK: [option/ir*4/fsr*4];
9uS7 3:12e1f116ea42 162 //option
9uS7 3:12e1f116ea42 163 if( buf[0]!=SYNC_SENSOR ){
9uS7 3:12e1f116ea42 164 return;
9uS7 3:12e1f116ea42 165 }
9uS7 3:12e1f116ea42 166
9uS7 3:12e1f116ea42 167 //ir
9uS7 3:12e1f116ea42 168 for( int i=0 ; i<4 ; i++ ){
9uS7 3:12e1f116ea42 169 temp.byte[i]=buf[1+i];
9uS7 3:12e1f116ea42 170 }
9uS7 3:12e1f116ea42 171 *_ir = temp.fl;
9uS7 3:12e1f116ea42 172
9uS7 3:12e1f116ea42 173 //fsr
9uS7 3:12e1f116ea42 174 for( int i=0 ; i<4 ; i++ ){
9uS7 3:12e1f116ea42 175 temp.byte[i] = buf[5+i];
9uS7 3:12e1f116ea42 176 }
9uS7 3:12e1f116ea42 177 *_fsr = temp.fl;
9uS7 3:12e1f116ea42 178
9uS7 3:12e1f116ea42 179 }