LALALA / Mbed 2 deprecated mbed_bluetooth

test

Dependencies:   mbed Motordriver SRF05

main.cpp@4:79adbd22ee27, 2016-12-10 (annotated)

Committer:
brian12858
Date:
Sat Dec 10 11:21:42 2016 +0000
Revision:
4:79adbd22ee27
Parent:
3:6484736ff515 
almost finish, front ultrasonic sensor not in use

Who changed what in which revision?

UserRevisionLine numberNew contents of line
brian12858 0:6d65c70ef417 1 #include "mbed.h"
brian12858 0:6d65c70ef417 2 #include <string>
brian12858 0:6d65c70ef417 3 #include <cstring>
brian12858 3:6484736ff515 4 //#include "rtos.h"
brian12858 4:79adbd22ee27 5 #include "SRF05.h"
brian12858 3:6484736ff515 6 #include "motordriver.h"
brian12858 0:6d65c70ef417 7 Serial bt(p28,p27);//tx,rx
brian12858 0:6d65c70ef417 8 Serial TAG(p9,p10);//tx,rx
brian12858 0:6d65c70ef417 9 Serial pc(USBTX,USBRX);
brian12858 0:6d65c70ef417 10 using namespace std;
brian12858 0:6d65c70ef417 11
brian12858 0:6d65c70ef417 12 void read_line();
brian12858 0:6d65c70ef417 13 void Rx_interrupt();
brian12858 3:6484736ff515 14 void move(char in);
brian12858 0:6d65c70ef417 15 // Circular buffers for serial TX and RX data - used by interrupt routines
brian12858 0:6d65c70ef417 16 const int buffer_size = 255;
brian12858 0:6d65c70ef417 17 // might need to increase buffer size for high baud rates
brian12858 0:6d65c70ef417 18 char rx_buffer[buffer_size+1];
brian12858 0:6d65c70ef417 19 volatile int rx_in=0;
brian12858 0:6d65c70ef417 20 volatile int rx_out=0;
brian12858 0:6d65c70ef417 21 char rx_line[80];
brian12858 0:6d65c70ef417 22
LALALALALALALALA 2:ff7c5f9ba765 23 string tagStr1, tagStr2, tagStr3;
LALALALALALALALA 2:ff7c5f9ba765 24 bool flag1=false, flag2=false, flag3=false;
LALALALALALALALA 2:ff7c5f9ba765 25 string str;
LALALALALALALALA 2:ff7c5f9ba765 26
brian12858 3:6484736ff515 27 Motor L(p21, p23, p22, 1); // pwm, fwd, rev, can brake ,right
brian12858 3:6484736ff515 28 Motor R(p26, p24, p25, 1); // pwm, fwd, rev, can brake ,left
brian12858 4:79adbd22ee27 29 const double speed=0.55;
brian12858 4:79adbd22ee27 30 const double speed_R = 0.85 * speed;
brian12858 4:79adbd22ee27 31 const double speed_L = 1 * speed;
brian12858 4:79adbd22ee27 32 SRF05 srfF(p11, p12);//Trigger,echo
brian12858 4:79adbd22ee27 33 SRF05 srfL(p13, p14);
brian12858 4:79adbd22ee27 34 SRF05 srfR(p7, p8);
brian12858 3:6484736ff515 35
brian12858 4:79adbd22ee27 36 int auto_move() {
brian12858 4:79adbd22ee27 37 //int d=40;
brian12858 4:79adbd22ee27 38 double sp_R=0;
brian12858 4:79adbd22ee27 39 double sp_L=0;
brian12858 4:79adbd22ee27 40
brian12858 4:79adbd22ee27 41
brian12858 4:79adbd22ee27 42 double r=srfR.read();
brian12858 4:79adbd22ee27 43 double l=srfL.read();
brian12858 4:79adbd22ee27 44 double w=(r+l)/2;
brian12858 4:79adbd22ee27 45
brian12858 4:79adbd22ee27 46 pc.printf("r = %f l=%f\n\r",r,l);
brian12858 4:79adbd22ee27 47 /*while(1){
brian12858 4:79adbd22ee27 48
brian12858 4:79adbd22ee27 49 printf("Distancef = %.1f\n",srfF.read());
brian12858 4:79adbd22ee27 50 printf("DistanceR = %.1f\n",srfR.read());
brian12858 4:79adbd22ee27 51 printf("DistanceL = %.1f\n",srfL.read());
brian12858 4:79adbd22ee27 52 }*/
brian12858 3:6484736ff515 53
brian12858 4:79adbd22ee27 54 //==============go straight beside the wall========================//
brian12858 4:79adbd22ee27 55
brian12858 4:79adbd22ee27 56 sp_R=speed_R*l/w;
brian12858 4:79adbd22ee27 57 sp_L=speed_L*r/w;
brian12858 4:79adbd22ee27 58 R.speed(sp_R),L.speed(sp_L);
brian12858 4:79adbd22ee27 59 //=======================================================//
brian12858 4:79adbd22ee27 60
brian12858 4:79adbd22ee27 61 //==================avoid obstacle=======================//
brian12858 4:79adbd22ee27 62 /**********************************************************
brian12858 4:79adbd22ee27 63 if(srfF.read()<=80)
brian12858 4:79adbd22ee27 64 {
brian12858 4:79adbd22ee27 65 R.speed(-0.6),L.speed(-0.7);//<80 lower speed
brian12858 4:79adbd22ee27 66
brian12858 4:79adbd22ee27 67 if(srfF.read()<=20)
brian12858 4:79adbd22ee27 68 { R.stop(-0.5),L.stop(-0.5);
brian12858 4:79adbd22ee27 69
brian12858 4:79adbd22ee27 70 if(srfR.read()>=80)
brian12858 4:79adbd22ee27 71 {
brian12858 4:79adbd22ee27 72 wait(0.2);
brian12858 4:79adbd22ee27 73 R.speed(-0.6),L.speed(-0.9);//turn right
brian12858 4:79adbd22ee27 74 if(srfF.read()>=80)
brian12858 4:79adbd22ee27 75 {
brian12858 4:79adbd22ee27 76 R.speed(-0.9),L.speed(-1);//forward
brian12858 4:79adbd22ee27 77 }
brian12858 4:79adbd22ee27 78 }
brian12858 4:79adbd22ee27 79 else
brian12858 4:79adbd22ee27 80 {
brian12858 4:79adbd22ee27 81 wait(0.2);
brian12858 4:79adbd22ee27 82 R.speed(-0.7),L.speed(-0.5);//turn left
brian12858 4:79adbd22ee27 83 if(srfF.read()>=80)
brian12858 4:79adbd22ee27 84 {
brian12858 4:79adbd22ee27 85 R.speed(-0.9),L.speed(-1);//forward
brian12858 4:79adbd22ee27 86 }
brian12858 4:79adbd22ee27 87 }
brian12858 4:79adbd22ee27 88 }
brian12858 4:79adbd22ee27 89 }
brian12858 4:79adbd22ee27 90 //**********************************************************/
brian12858 4:79adbd22ee27 91 //============================================================//
brian12858 4:79adbd22ee27 92 }
brian12858 3:6484736ff515 93 void move(char in){
brian12858 3:6484736ff515 94 char ch[1]={'s'};
brian12858 4:79adbd22ee27 95 ch[0] = in;//in;
brian12858 3:6484736ff515 96 // while(1) {
brian12858 3:6484736ff515 97 //pc.printf("ch[0]= %s\n\r",ch);
brian12858 3:6484736ff515 98 /* if(bt.readable ()){
brian12858 3:6484736ff515 99 ch[0]=bt.getc();
brian12858 3:6484736ff515 100 //pc.printf("string1 = %s\n\r",ch);
brian12858 3:6484736ff515 101 } */
brian12858 3:6484736ff515 102 switch(ch[0]){
brian12858 3:6484736ff515 103 case 'w':
brian12858 3:6484736ff515 104 //前進
brian12858 4:79adbd22ee27 105 R.speed(speed_R),L.speed(speed_L);
brian12858 3:6484736ff515 106 break;
brian12858 3:6484736ff515 107 case 'a':
brian12858 3:6484736ff515 108 //左轉
brian12858 4:79adbd22ee27 109 R.speed(speed_R),L.speed(0);
brian12858 3:6484736ff515 110 break;
brian12858 3:6484736ff515 111 case 's':
brian12858 3:6484736ff515 112 //停住
brian12858 3:6484736ff515 113 R.stop(0.3),L.stop(0.3);
brian12858 3:6484736ff515 114 break;
brian12858 3:6484736ff515 115 case 'd':
brian12858 3:6484736ff515 116 //右轉
brian12858 4:79adbd22ee27 117 R.speed(0),L.speed(speed_L);
brian12858 3:6484736ff515 118 break;
brian12858 3:6484736ff515 119 case 'x':
brian12858 3:6484736ff515 120 //backward
brian12858 4:79adbd22ee27 121 R.speed(-speed_R),L.speed(-speed_L);
brian12858 3:6484736ff515 122 break;
brian12858 3:6484736ff515 123 case 'm':
brian12858 3:6484736ff515 124 //自走車
brian12858 4:79adbd22ee27 125 pc.printf("auto move");
brian12858 4:79adbd22ee27 126 auto_move();
brian12858 3:6484736ff515 127 break;
brian12858 3:6484736ff515 128 }
brian12858 4:79adbd22ee27 129 // }
brian12858 3:6484736ff515 130 }
brian12858 0:6d65c70ef417 131 //std::string.
brian12858 4:79adbd22ee27 132 int main() {
brian12858 4:79adbd22ee27 133
brian12858 0:6d65c70ef417 134 bt.baud(38400);
brian12858 0:6d65c70ef417 135 pc.baud(9600);
brian12858 0:6d65c70ef417 136 TAG.baud(115200);
brian12858 0:6d65c70ef417 137 pc.printf("pc hello\n\r");
brian12858 0:6d65c70ef417 138 bt.printf("bt hello\n\r");
brian12858 0:6d65c70ef417 139
LALALALALALALALA 2:ff7c5f9ba765 140 TAG.attach(&Rx_interrupt, Serial::RxIrq);
brian12858 3:6484736ff515 141 char ch = 's' ;
brian12858 3:6484736ff515 142 //Thread thread(move_thread);
brian12858 3:6484736ff515 143 //thread.set_priority(osPriorityLow);
brian12858 3:6484736ff515 144
brian12858 0:6d65c70ef417 145 while(1) {
brian12858 4:79adbd22ee27 146
LALALALALALALALA 2:ff7c5f9ba765 147
LALALALALALALALA 2:ff7c5f9ba765 148 // Read a line from the large rx buffer from rx interrupt routine
LALALALALALALALA 2:ff7c5f9ba765 149 read_line();
LALALALALALALALA 2:ff7c5f9ba765 150 // Read ASCII number from rx line buffer
LALALALALALALALA 2:ff7c5f9ba765 151 //sscanf(rx_line,"%d",&rx_i);
LALALALALALALALA 2:ff7c5f9ba765 152 //pc.printf("string = %s\n\r",rx_line);
LALALALALALALALA 2:ff7c5f9ba765 153 // 若收到藍牙模組的資料，則送到「序列埠監控視窗」
LALALALALALALALA 2:ff7c5f9ba765 154 //pc.printf("tag thread.");
LALALALALALALALA 2:ff7c5f9ba765 155 switch(rx_line[1]){
LALALALALALALALA 2:ff7c5f9ba765 156 case '0':
LALALALALALALALA 2:ff7c5f9ba765 157 flag1=true;
LALALALALALALALA 2:ff7c5f9ba765 158 tagStr1=&rx_line[2];
brian12858 4:79adbd22ee27 159 //pc.printf("string1 = %s\n\r",tagStr1);
LALALALALALALALA 2:ff7c5f9ba765 160 break;
LALALALALALALALA 2:ff7c5f9ba765 161 case '1':
LALALALALALALALA 2:ff7c5f9ba765 162 flag2=true;
LALALALALALALALA 2:ff7c5f9ba765 163 tagStr2=&rx_line[2];
brian12858 4:79adbd22ee27 164 //pc.printf("string2 = %s\n\r",tagStr2);
LALALALALALALALA 2:ff7c5f9ba765 165 break;
LALALALALALALALA 2:ff7c5f9ba765 166 case '2':
LALALALALALALALA 2:ff7c5f9ba765 167 flag3=true;
LALALALALALALALA 2:ff7c5f9ba765 168 tagStr3=&rx_line[2];
brian12858 4:79adbd22ee27 169 //pc.printf("string3 = %s\n\r",tagStr3);
LALALALALALALALA 2:ff7c5f9ba765 170 break;
brian12858 0:6d65c70ef417 171 }
LALALALALALALALA 2:ff7c5f9ba765 172 if(flag3==true&&flag2==true&&flag1==true){
LALALALALALALALA 2:ff7c5f9ba765 173 flag1=false,flag2=false,flag3=false;
LALALALALALALALA 2:ff7c5f9ba765 174 str=tagStr1+","+tagStr2+","+tagStr3;
LALALALALALALALA 2:ff7c5f9ba765 175 bt.printf("%s\n",str);
brian12858 0:6d65c70ef417 176 }
LALALALALALALALA 2:ff7c5f9ba765 177
brian12858 3:6484736ff515 178 if(bt.readable())
brian12858 3:6484736ff515 179 ch=bt.getc();
brian12858 3:6484736ff515 180 move(ch);
brian12858 3:6484736ff515 181
brian12858 3:6484736ff515 182
brian12858 3:6484736ff515 183
brian12858 3:6484736ff515 184
brian12858 3:6484736ff515 185
brian12858 0:6d65c70ef417 186 }
brian12858 0:6d65c70ef417 187 }
brian12858 0:6d65c70ef417 188 // Read a line from the large rx buffer from rx interrupt routine
brian12858 0:6d65c70ef417 189 void read_line() {
brian12858 0:6d65c70ef417 190 int i;
brian12858 0:6d65c70ef417 191 i = 0;
brian12858 0:6d65c70ef417 192 // Start Critical Section - don't interrupt while changing global buffer variables
brian12858 0:6d65c70ef417 193 NVIC_DisableIRQ(UART1_IRQn);
brian12858 0:6d65c70ef417 194 // Loop reading rx buffer characters until end of line character
brian12858 0:6d65c70ef417 195 while ((i==0) || (rx_line[i-1] != '\r')) {
brian12858 0:6d65c70ef417 196 // Wait if buffer empty
brian12858 0:6d65c70ef417 197 if (rx_in == rx_out) {
brian12858 0:6d65c70ef417 198 // End Critical Section - need to allow rx interrupt to get new characters for buffer
brian12858 0:6d65c70ef417 199 NVIC_EnableIRQ(UART1_IRQn);
brian12858 0:6d65c70ef417 200 while (rx_in == rx_out) {
brian12858 0:6d65c70ef417 201 }
brian12858 0:6d65c70ef417 202 // Start Critical Section - don't interrupt while changing global buffer variables
brian12858 0:6d65c70ef417 203 NVIC_DisableIRQ(UART1_IRQn);
brian12858 0:6d65c70ef417 204 }
brian12858 0:6d65c70ef417 205 rx_line[i] = rx_buffer[rx_out];
brian12858 0:6d65c70ef417 206 i++;
brian12858 0:6d65c70ef417 207 rx_out = (rx_out + 1) % buffer_size;
brian12858 0:6d65c70ef417 208 }
brian12858 0:6d65c70ef417 209 // End Critical Section
brian12858 0:6d65c70ef417 210 NVIC_EnableIRQ(UART1_IRQn);
brian12858 0:6d65c70ef417 211 rx_line[i-1] = 0;
brian12858 0:6d65c70ef417 212 return;
brian12858 0:6d65c70ef417 213 }
brian12858 0:6d65c70ef417 214
brian12858 0:6d65c70ef417 215 // Interupt Routine to read in data from serial port
brian12858 0:6d65c70ef417 216 void Rx_interrupt() {
brian12858 0:6d65c70ef417 217 // Loop just in case more than one character is in UART's receive FIFO buffer
brian12858 0:6d65c70ef417 218 // Stop if buffer full
brian12858 0:6d65c70ef417 219 while ((TAG.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
brian12858 0:6d65c70ef417 220 rx_buffer[rx_in] = TAG.getc();
brian12858 0:6d65c70ef417 221 // Uncomment to Echo to USB serial to watch data flow
brian12858 0:6d65c70ef417 222 // monitor_TAG.putc(rx_buffer[rx_in]);
brian12858 0:6d65c70ef417 223 rx_in = (rx_in + 1) % buffer_size;
brian12858 0:6d65c70ef417 224 }
brian12858 0:6d65c70ef417 225 return;
brian12858 0:6d65c70ef417 226 }