Ohnishi_Gundan / Mbed 2 deprecated Master-FM

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MODSERIAL mbed-rtos mbed

Fork of Master by Ohnishi_Gundan

bluetooth.cpp@9:6057314dc8ec, 2014-09-13 (annotated)

Committer:
9uS7
Date:
Sat Sep 13 11:03:52 2014 +0000
Revision:
9:6057314dc8ec
Parent:
8:bfcfda6b38fe
Child:
10:a90935ea0a4b
can control motor

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 3:12e1f116ea42 31 void sync(char option, char* b_data, float* f_data)
9uS7 2:c610e1a7fbcd 32 {
9uS7 4:aaaadb45cbd9 33 char pac[PACK_SIZE]={};
9uS7 3:12e1f116ea42 34 Cvt temp;
9uS7 3:12e1f116ea42 35
9uS7 3:12e1f116ea42 36 //making pac
9uS7 8:bfcfda6b38fe 37 pac[0]=option;
9uS7 3:12e1f116ea42 38 if( option==SYNC_MOTOR ){
9uS7 3:12e1f116ea42 39 //PACK: [option/function/pwm*4]
9uS7 3:12e1f116ea42 40 //function
9uS7 3:12e1f116ea42 41 pac[1]=b_data[0];
9uS7 3:12e1f116ea42 42 //pwm
9uS7 3:12e1f116ea42 43 temp.fl = f_data[0];
9uS7 3:12e1f116ea42 44 for( int i=0 ; i<4 ; i++ ){
9uS7 3:12e1f116ea42 45 pac[2+i] = temp.byte[i];
9uS7 3:12e1f116ea42 46 }
9uS7 3:12e1f116ea42 47 }
9uS7 3:12e1f116ea42 48 else if( option==SYNC_FM ){
9uS7 3:12e1f116ea42 49 //PACK: [option/request]
9uS7 3:12e1f116ea42 50 pac[1]=b_data[0]; //request
9uS7 1:e1cfb5850088 51 }
9uS7 2:c610e1a7fbcd 52 else{
9uS7 3:12e1f116ea42 53 ;
9uS7 3:12e1f116ea42 54 }
9uS7 3:12e1f116ea42 55
9uS7 3:12e1f116ea42 56 //send pac
9uS7 3:12e1f116ea42 57 for( int i=0 ; i<PACK_SIZE ; i++ ){
9uS7 4:aaaadb45cbd9 58 bt.putc( pac[i] );
9uS7 4:aaaadb45cbd9 59 pc.printf("%02x ",pac[i]);
9uS7 2:c610e1a7fbcd 60 }
9uS7 4:aaaadb45cbd9 61 pc.printf("\n");
9uS7 2:c610e1a7fbcd 62 }
9uS7 3:12e1f116ea42 63
9uS7 3:12e1f116ea42 64 void slaveRecieve(void)
9uS7 3:12e1f116ea42 65 {
9uS7 4:aaaadb45cbd9 66 static int i=0;
9uS7 4:aaaadb45cbd9 67 char buf[PACK_SIZE]={};
9uS7 4:aaaadb45cbd9 68 char pac[PACK_SIZE]={};
9uS7 4:aaaadb45cbd9 69 float val[PACK_SIZE/4+1]={};
9uS7 3:12e1f116ea42 70 Cvt temp;
9uS7 3:12e1f116ea42 71
9uS7 3:12e1f116ea42 72 wait(1/1000.0);
9uS7 3:12e1f116ea42 73
9uS7 8:bfcfda6b38fe 74 //l1=( l1 ? 0 : 1 );
9uS7 8:bfcfda6b38fe 75
9uS7 8:bfcfda6b38fe 76 l4=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 8:bfcfda6b38fe 80 }
9uS7 8:bfcfda6b38fe 81
9uS7 8:bfcfda6b38fe 82 l3=1;
9uS7 3:12e1f116ea42 83
9uS7 3:12e1f116ea42 84 if( buf[0]==SYNC_MOTOR ){
9uS7 3:12e1f116ea42 85 //PACK: [option/function/pwm*4]
9uS7 3:12e1f116ea42 86 //pwm
9uS7 3:12e1f116ea42 87 for( int i=0 ; i<4 ; i++ ){
9uS7 9:6057314dc8ec 88 temp.byte[i]=buf[2+i];
9uS7 3:12e1f116ea42 89 }
9uS7 3:12e1f116ea42 90 motor( buf[1], temp.fl );
9uS7 3:12e1f116ea42 91 }
9uS7 3:12e1f116ea42 92 else if( buf[0]==SYNC_FM ){
9uS7 3:12e1f116ea42 93 //PACK: [option/request]
9uS7 3:12e1f116ea42 94 ; //not yet
9uS7 3:12e1f116ea42 95 }
9uS7 8:bfcfda6b38fe 96 else if( buf[0]==SYNC_SENSOR ){
9uS7 8:bfcfda6b38fe 97 l2=1;
9uS7 3:12e1f116ea42 98 getSensor( &(val[0]), &(val[1]) );
9uS7 3:12e1f116ea42 99 //PACK: [option/ir*4/fsr*4];
9uS7 3:12e1f116ea42 100 //option
9uS7 3:12e1f116ea42 101 pac[0] = SYNC_SENSOR;
9uS7 3:12e1f116ea42 102 //ir
9uS7 3:12e1f116ea42 103 temp.fl = val[0];
9uS7 3:12e1f116ea42 104 for( int i=0 ; i<4 ; i++ ){
9uS7 3:12e1f116ea42 105 pac[1+i] = temp.byte[i];
9uS7 3:12e1f116ea42 106 }
9uS7 3:12e1f116ea42 107 //fsr
9uS7 3:12e1f116ea42 108 temp.fl = val[1];
9uS7 3:12e1f116ea42 109 for( int i=0 ; i<4 ; i++ ){
9uS7 3:12e1f116ea42 110 pac[5+i] = temp.byte[i];
9uS7 3:12e1f116ea42 111 }
9uS7 8:bfcfda6b38fe 112 pc.printf( "%f\n", val[1] );
9uS7 3:12e1f116ea42 113 //send pac
9uS7 3:12e1f116ea42 114 for( int i=0 ; i<PACK_SIZE ; i++ ){
9uS7 3:12e1f116ea42 115 bt.putc( pac[i] );
9uS7 4:aaaadb45cbd9 116 pc.printf("%02x ",pac[i]);
9uS7 3:12e1f116ea42 117 }
9uS7 4:aaaadb45cbd9 118 pc.printf("\n");
9uS7 4:aaaadb45cbd9 119
9uS7 4:aaaadb45cbd9 120 l2 = l2 ? 0 : 1;
9uS7 3:12e1f116ea42 121 }
9uS7 3:12e1f116ea42 122 else if( buf[0]==SYNC_FM ){
9uS7 3:12e1f116ea42 123 ; //not yet
9uS7 3:12e1f116ea42 124
9uS7 3:12e1f116ea42 125 //send pac
9uS7 3:12e1f116ea42 126 for( int i=0 ; i<PACK_SIZE ; i++ ){
9uS7 3:12e1f116ea42 127 bt.putc( pac[i] );
9uS7 3:12e1f116ea42 128 }
9uS7 3:12e1f116ea42 129 }
9uS7 3:12e1f116ea42 130 }
9uS7 3:12e1f116ea42 131
9uS7 6:df6d8ba1907a 132 void recieveSensor(float* _ir, float* _fsr)
9uS7 3:12e1f116ea42 133 {
9uS7 4:aaaadb45cbd9 134 char buf[PACK_SIZE]={};
9uS7 3:12e1f116ea42 135 Cvt temp;
9uS7 3:12e1f116ea42 136
9uS7 8:bfcfda6b38fe 137 sync(SYNC_SENSOR,NULL,NULL);
9uS7 8:bfcfda6b38fe 138 wait(0.0001);
9uS7 4:aaaadb45cbd9 139
9uS7 4:aaaadb45cbd9 140 l1= l1 ? 0 : 1;
9uS7 8:bfcfda6b38fe 141
9uS7 8:bfcfda6b38fe 142 //Read
9uS7 8:bfcfda6b38fe 143 for(int i=0 ;;){
9uS7 4:aaaadb45cbd9 144 if( bt.readable() ){
9uS7 8:bfcfda6b38fe 145 buf[0]=bt.getc();
9uS7 4:aaaadb45cbd9 146 pc.printf("readable\n");
9uS7 8:bfcfda6b38fe 147 for( int j=1 ; j<PACK_SIZE ;){
9uS7 8:bfcfda6b38fe 148 if(bt.readable()){
9uS7 8:bfcfda6b38fe 149 buf[j]=bt.getc();
9uS7 8:bfcfda6b38fe 150 j++;
9uS7 8:bfcfda6b38fe 151 }else{
9uS7 8:bfcfda6b38fe 152 pc.printf("hoge:%d\n",j);
9uS7 8:bfcfda6b38fe 153 break;
9uS7 8:bfcfda6b38fe 154 }
9uS7 8:bfcfda6b38fe 155 }
9uS7 4:aaaadb45cbd9 156 break;
9uS7 4:aaaadb45cbd9 157 }
9uS7 8:bfcfda6b38fe 158 else{
9uS7 8:bfcfda6b38fe 159 i++;
9uS7 8:bfcfda6b38fe 160 if( i>5 ){
9uS7 8:bfcfda6b38fe 161 l4 = 0;
9uS7 8:bfcfda6b38fe 162 pc.printf("not readable\n");
9uS7 8:bfcfda6b38fe 163 return;
9uS7 8:bfcfda6b38fe 164 }
9uS7 4:aaaadb45cbd9 165 }
9uS7 4:aaaadb45cbd9 166 }
9uS7 4:aaaadb45cbd9 167
9uS7 8:bfcfda6b38fe 168
9uS7 3:12e1f116ea42 169 for( int i=0 ; i<PACK_SIZE ; i++ ){
9uS7 8:bfcfda6b38fe 170 pc.printf("%02x ",buf[i]);
9uS7 3:12e1f116ea42 171 }
9uS7 8:bfcfda6b38fe 172 pc.printf("\n");
9uS7 8:bfcfda6b38fe 173 l4 = 1;
9uS7 4:aaaadb45cbd9 174
9uS7 3:12e1f116ea42 175 //PACK: [option/ir*4/fsr*4];
9uS7 3:12e1f116ea42 176 //option
9uS7 3:12e1f116ea42 177 if( buf[0]!=SYNC_SENSOR ){
9uS7 3:12e1f116ea42 178 return;
9uS7 3:12e1f116ea42 179 }
9uS7 3:12e1f116ea42 180
9uS7 3:12e1f116ea42 181 //ir
9uS7 3:12e1f116ea42 182 for( int i=0 ; i<4 ; i++ ){
9uS7 3:12e1f116ea42 183 temp.byte[i]=buf[1+i];
9uS7 3:12e1f116ea42 184 }
9uS7 3:12e1f116ea42 185 *_ir = temp.fl;
9uS7 3:12e1f116ea42 186
9uS7 3:12e1f116ea42 187 //fsr
9uS7 3:12e1f116ea42 188 for( int i=0 ; i<4 ; i++ ){
9uS7 3:12e1f116ea42 189 temp.byte[i] = buf[5+i];
9uS7 3:12e1f116ea42 190 }
9uS7 3:12e1f116ea42 191 *_fsr = temp.fl;
9uS7 8:bfcfda6b38fe 192
9uS7 8:bfcfda6b38fe 193 pc.printf( "%f %f\n", *_ir, *_fsr );
9uS7 3:12e1f116ea42 194
9uS7 9:6057314dc8ec 195 }
9uS7 9:6057314dc8ec 196
9uS7 9:6057314dc8ec 197 void sendMotor( char function, float power ){
9uS7 9:6057314dc8ec 198 char f[1];
9uS7 9:6057314dc8ec 199 float p[1];
9uS7 9:6057314dc8ec 200 f[0] = function;
9uS7 9:6057314dc8ec 201 p[0] = power;
9uS7 9:6057314dc8ec 202 sync( SYNC_MOTOR, f, p );
9uS7 3:12e1f116ea42 203 }