Ohnishi_Gundan / Mbed 2 deprecated Master-FM

FM-test

Dependencies:   MODSERIAL mbed-rtos mbed

Fork of Master by Ohnishi_Gundan

bluetooth.cpp@13:3f0505bbe284, 2014-09-15 (annotated)

Committer:
9uS7
Date:
Mon Sep 15 04:05:56 2014 +0000
Revision:
13:3f0505bbe284
Parent:
10:a90935ea0a4b 
add fmStop and fmRestart

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 8:bfcfda6b38fe 4 #include "RawSerial.h"
9uS7 0:4f07ba929908 5
9uS7 8:bfcfda6b38fe 6 RawSerial bt(p28, p27); // tx, rx
9uS7 3:12e1f116ea42 7
9uS7 8:bfcfda6b38fe 8 RawSerial pc(USBTX, USBRX); // tx, rx
9uS7 4:aaaadb45cbd9 9
9uS7 3:12e1f116ea42 10 DigitalOut l1(LED1);
9uS7 4:aaaadb45cbd9 11 DigitalOut l2(LED2);
9uS7 8:bfcfda6b38fe 12 DigitalOut l3(LED3);
9uS7 4:aaaadb45cbd9 13 DigitalOut l4(LED4);
9uS7 0:4f07ba929908 14
9uS7 1:e1cfb5850088 15 void btSetup(int role)
9uS7 0:4f07ba929908 16 {
9uS7 1:e1cfb5850088 17 if( role==BT_MASTER ){
9uS7 1:e1cfb5850088 18 //if this device is the master
9uS7 1:e1cfb5850088 19 bt.baud(9600);
9uS7 1:e1cfb5850088 20 bt.printf("$$$");
9uS7 1:e1cfb5850088 21 wait(0.5);
9uS7 1:e1cfb5850088 22 bt.printf("C\r");
9uS7 1:e1cfb5850088 23 wait(0.5);
9uS7 1:e1cfb5850088 24 }
9uS7 1:e1cfb5850088 25 else{
9uS7 1:e1cfb5850088 26 //if this device is the slave
9uS7 3:12e1f116ea42 27 bt.attach( slaveRecieve, Serial::RxIrq );
9uS7 3:12e1f116ea42 28 }
9uS7 2:c610e1a7fbcd 29 }
9uS7 3:12e1f116ea42 30
9uS7 10:a90935ea0a4b 31 int readPack( char* buf ){
9uS7 10:a90935ea0a4b 32 int error=0;
9uS7 10:a90935ea0a4b 33 for(int i=0 ;;){
9uS7 10:a90935ea0a4b 34 if( bt.readable() ){
9uS7 10:a90935ea0a4b 35 buf[0]=bt.getc();
9uS7 10:a90935ea0a4b 36 pc.printf("readable\n");
9uS7 10:a90935ea0a4b 37 for( int j=1 ; j<PACK_SIZE ;){
9uS7 10:a90935ea0a4b 38 if(bt.readable()){
9uS7 10:a90935ea0a4b 39 buf[j]=bt.getc();
9uS7 10:a90935ea0a4b 40 j++;
9uS7 10:a90935ea0a4b 41 }else{
9uS7 10:a90935ea0a4b 42 pc.printf("hoge:%d\n",j);
9uS7 10:a90935ea0a4b 43 error = j;
9uS7 10:a90935ea0a4b 44 break;
9uS7 10:a90935ea0a4b 45 }
9uS7 10:a90935ea0a4b 46 }
9uS7 10:a90935ea0a4b 47 break;
9uS7 10:a90935ea0a4b 48 }
9uS7 10:a90935ea0a4b 49 else{
9uS7 10:a90935ea0a4b 50 i++;
9uS7 10:a90935ea0a4b 51 if( i>5 ){
9uS7 10:a90935ea0a4b 52 l4 = 0;
9uS7 10:a90935ea0a4b 53 pc.printf("not readable\n");
9uS7 10:a90935ea0a4b 54 return -1;
9uS7 10:a90935ea0a4b 55 }
9uS7 10:a90935ea0a4b 56 }
9uS7 10:a90935ea0a4b 57 }
9uS7 10:a90935ea0a4b 58 return error;
9uS7 10:a90935ea0a4b 59 }
9uS7 10:a90935ea0a4b 60
9uS7 3:12e1f116ea42 61 void sync(char option, char* b_data, float* f_data)
9uS7 2:c610e1a7fbcd 62 {
9uS7 4:aaaadb45cbd9 63 char pac[PACK_SIZE]={};
9uS7 3:12e1f116ea42 64 Cvt temp;
9uS7 3:12e1f116ea42 65
9uS7 3:12e1f116ea42 66 //making pac
9uS7 8:bfcfda6b38fe 67 pac[0]=option;
9uS7 3:12e1f116ea42 68 if( option==SYNC_MOTOR ){
9uS7 3:12e1f116ea42 69 //PACK: [option/function/pwm*4]
9uS7 3:12e1f116ea42 70 //function
9uS7 3:12e1f116ea42 71 pac[1]=b_data[0];
9uS7 3:12e1f116ea42 72 //pwm
9uS7 3:12e1f116ea42 73 temp.fl = f_data[0];
9uS7 3:12e1f116ea42 74 for( int i=0 ; i<4 ; i++ ){
9uS7 3:12e1f116ea42 75 pac[2+i] = temp.byte[i];
9uS7 3:12e1f116ea42 76 }
9uS7 3:12e1f116ea42 77 }
9uS7 3:12e1f116ea42 78 else if( option==SYNC_FM ){
9uS7 3:12e1f116ea42 79 //PACK: [option/request]
9uS7 3:12e1f116ea42 80 pac[1]=b_data[0]; //request
9uS7 1:e1cfb5850088 81 }
9uS7 2:c610e1a7fbcd 82 else{
9uS7 3:12e1f116ea42 83 ;
9uS7 3:12e1f116ea42 84 }
9uS7 3:12e1f116ea42 85
9uS7 3:12e1f116ea42 86 //send pac
9uS7 3:12e1f116ea42 87 for( int i=0 ; i<PACK_SIZE ; i++ ){
9uS7 4:aaaadb45cbd9 88 bt.putc( pac[i] );
9uS7 4:aaaadb45cbd9 89 pc.printf("%02x ",pac[i]);
9uS7 2:c610e1a7fbcd 90 }
9uS7 4:aaaadb45cbd9 91 pc.printf("\n");
9uS7 2:c610e1a7fbcd 92 }
9uS7 3:12e1f116ea42 93
9uS7 3:12e1f116ea42 94 void slaveRecieve(void)
9uS7 3:12e1f116ea42 95 {
9uS7 10:a90935ea0a4b 96 //static int i=0;
9uS7 4:aaaadb45cbd9 97 char buf[PACK_SIZE]={};
9uS7 4:aaaadb45cbd9 98 char pac[PACK_SIZE]={};
9uS7 10:a90935ea0a4b 99 int read_error;
9uS7 4:aaaadb45cbd9 100 float val[PACK_SIZE/4+1]={};
9uS7 3:12e1f116ea42 101 Cvt temp;
9uS7 3:12e1f116ea42 102
9uS7 3:12e1f116ea42 103 wait(1/1000.0);
9uS7 3:12e1f116ea42 104
9uS7 8:bfcfda6b38fe 105 //l1=( l1 ? 0 : 1 );
9uS7 8:bfcfda6b38fe 106
9uS7 8:bfcfda6b38fe 107 l4=1;
9uS7 3:12e1f116ea42 108
9uS7 10:a90935ea0a4b 109 //read_error = readPack( buf );
9uS7 10:a90935ea0a4b 110 //if( read_error ) return;
9uS7 10:a90935ea0a4b 111
9uS7 3:12e1f116ea42 112 for( int i=0 ; i<PACK_SIZE ; i++ ){
9uS7 3:12e1f116ea42 113 buf[i]=bt.getc();
9uS7 8:bfcfda6b38fe 114 }
9uS7 8:bfcfda6b38fe 115
9uS7 8:bfcfda6b38fe 116 l3=1;
9uS7 3:12e1f116ea42 117
9uS7 3:12e1f116ea42 118 if( buf[0]==SYNC_MOTOR ){
9uS7 3:12e1f116ea42 119 //PACK: [option/function/pwm*4]
9uS7 3:12e1f116ea42 120 //pwm
9uS7 3:12e1f116ea42 121 for( int i=0 ; i<4 ; i++ ){
9uS7 9:6057314dc8ec 122 temp.byte[i]=buf[2+i];
9uS7 3:12e1f116ea42 123 }
9uS7 3:12e1f116ea42 124 motor( buf[1], temp.fl );
9uS7 3:12e1f116ea42 125 }
9uS7 3:12e1f116ea42 126 else if( buf[0]==SYNC_FM ){
9uS7 3:12e1f116ea42 127 //PACK: [option/request]
9uS7 3:12e1f116ea42 128 ; //not yet
9uS7 3:12e1f116ea42 129 }
9uS7 8:bfcfda6b38fe 130 else if( buf[0]==SYNC_SENSOR ){
9uS7 8:bfcfda6b38fe 131 l2=1;
9uS7 3:12e1f116ea42 132 getSensor( &(val[0]), &(val[1]) );
9uS7 3:12e1f116ea42 133 //PACK: [option/ir*4/fsr*4];
9uS7 3:12e1f116ea42 134 //option
9uS7 3:12e1f116ea42 135 pac[0] = SYNC_SENSOR;
9uS7 3:12e1f116ea42 136 //ir
9uS7 3:12e1f116ea42 137 temp.fl = val[0];
9uS7 3:12e1f116ea42 138 for( int i=0 ; i<4 ; i++ ){
9uS7 3:12e1f116ea42 139 pac[1+i] = temp.byte[i];
9uS7 3:12e1f116ea42 140 }
9uS7 3:12e1f116ea42 141 //fsr
9uS7 3:12e1f116ea42 142 temp.fl = val[1];
9uS7 3:12e1f116ea42 143 for( int i=0 ; i<4 ; i++ ){
9uS7 3:12e1f116ea42 144 pac[5+i] = temp.byte[i];
9uS7 3:12e1f116ea42 145 }
9uS7 8:bfcfda6b38fe 146 pc.printf( "%f\n", val[1] );
9uS7 3:12e1f116ea42 147 //send pac
9uS7 3:12e1f116ea42 148 for( int i=0 ; i<PACK_SIZE ; i++ ){
9uS7 3:12e1f116ea42 149 bt.putc( pac[i] );
9uS7 4:aaaadb45cbd9 150 pc.printf("%02x ",pac[i]);
9uS7 3:12e1f116ea42 151 }
9uS7 4:aaaadb45cbd9 152 pc.printf("\n");
9uS7 4:aaaadb45cbd9 153
9uS7 4:aaaadb45cbd9 154 l2 = l2 ? 0 : 1;
9uS7 3:12e1f116ea42 155 }
9uS7 3:12e1f116ea42 156 else if( buf[0]==SYNC_FM ){
9uS7 3:12e1f116ea42 157 ; //not yet
9uS7 3:12e1f116ea42 158
9uS7 3:12e1f116ea42 159 //send pac
9uS7 3:12e1f116ea42 160 for( int i=0 ; i<PACK_SIZE ; i++ ){
9uS7 3:12e1f116ea42 161 bt.putc( pac[i] );
9uS7 3:12e1f116ea42 162 }
9uS7 3:12e1f116ea42 163 }
9uS7 3:12e1f116ea42 164 }
9uS7 3:12e1f116ea42 165
9uS7 6:df6d8ba1907a 166 void recieveSensor(float* _ir, float* _fsr)
9uS7 3:12e1f116ea42 167 {
9uS7 4:aaaadb45cbd9 168 char buf[PACK_SIZE]={};
9uS7 10:a90935ea0a4b 169 int read_error;
9uS7 3:12e1f116ea42 170 Cvt temp;
9uS7 3:12e1f116ea42 171
9uS7 8:bfcfda6b38fe 172 sync(SYNC_SENSOR,NULL,NULL);
9uS7 8:bfcfda6b38fe 173 wait(0.0001);
9uS7 4:aaaadb45cbd9 174
9uS7 4:aaaadb45cbd9 175 l1= l1 ? 0 : 1;
9uS7 8:bfcfda6b38fe 176
9uS7 8:bfcfda6b38fe 177 //Read
9uS7 10:a90935ea0a4b 178 read_error=readPack( buf );
9uS7 10:a90935ea0a4b 179 if( read_error )return;
9uS7 8:bfcfda6b38fe 180
9uS7 3:12e1f116ea42 181 for( int i=0 ; i<PACK_SIZE ; i++ ){
9uS7 8:bfcfda6b38fe 182 pc.printf("%02x ",buf[i]);
9uS7 3:12e1f116ea42 183 }
9uS7 8:bfcfda6b38fe 184 pc.printf("\n");
9uS7 8:bfcfda6b38fe 185 l4 = 1;
9uS7 4:aaaadb45cbd9 186
9uS7 3:12e1f116ea42 187 //PACK: [option/ir*4/fsr*4];
9uS7 3:12e1f116ea42 188 //option
9uS7 3:12e1f116ea42 189 if( buf[0]!=SYNC_SENSOR ){
9uS7 3:12e1f116ea42 190 return;
9uS7 3:12e1f116ea42 191 }
9uS7 3:12e1f116ea42 192
9uS7 3:12e1f116ea42 193 //ir
9uS7 3:12e1f116ea42 194 for( int i=0 ; i<4 ; i++ ){
9uS7 3:12e1f116ea42 195 temp.byte[i]=buf[1+i];
9uS7 3:12e1f116ea42 196 }
9uS7 3:12e1f116ea42 197 *_ir = temp.fl;
9uS7 3:12e1f116ea42 198
9uS7 3:12e1f116ea42 199 //fsr
9uS7 3:12e1f116ea42 200 for( int i=0 ; i<4 ; i++ ){
9uS7 3:12e1f116ea42 201 temp.byte[i] = buf[5+i];
9uS7 3:12e1f116ea42 202 }
9uS7 3:12e1f116ea42 203 *_fsr = temp.fl;
9uS7 8:bfcfda6b38fe 204
9uS7 8:bfcfda6b38fe 205 pc.printf( "%f %f\n", *_ir, *_fsr );
9uS7 3:12e1f116ea42 206
9uS7 9:6057314dc8ec 207 }
9uS7 9:6057314dc8ec 208
9uS7 9:6057314dc8ec 209 void sendMotor( char function, float power ){
9uS7 9:6057314dc8ec 210 char f[1];
9uS7 9:6057314dc8ec 211 float p[1];
9uS7 9:6057314dc8ec 212 f[0] = function;
9uS7 9:6057314dc8ec 213 p[0] = power;
9uS7 9:6057314dc8ec 214 sync( SYNC_MOTOR, f, p );
9uS7 3:12e1f116ea42 215 }