Kang hokyoung / Mbed 2 deprecated 2014_Ensoul_Capstone

2014_Ensoul_Capstone

Dependencies:   TextLCD Ultrasonic mbed BufferedSoftSerial

main.cpp@3:59a400d203cc, 2014-07-03 (annotated)

Committer:
MR_Kang
Date:
Thu Jul 03 17:50:19 2014 +0000
Revision:
3:59a400d203cc
Parent:
2:c9740fccf3a7
Child:
4:fd36ff807a91
?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
leejong87 1:2859bfed20b4 1 //Developer Kang, Lee
leejong87 1:2859bfed20b4 2
MR_Kang 0:d429f13fe4be 3 #include "mbed.h"
MR_Kang 0:d429f13fe4be 4 #include "Ultrasonic.h"
MR_Kang 3:59a400d203cc 5 #include "BufferedSoftSerial.h"
MR_Kang 0:d429f13fe4be 6
leejong87 2:c9740fccf3a7 7 Serial xbee1(p9, p10); // tx, rx
leejong87 2:c9740fccf3a7 8 Serial xbee2(p13, p14);
leejong87 2:c9740fccf3a7 9 Serial xbee3(p28, p27);
MR_Kang 3:59a400d203cc 10 SoftSerial bt(p29,p30);
leejong87 2:c9740fccf3a7 11
MR_Kang 0:d429f13fe4be 12 AnalogIn Sharp(p20);
MR_Kang 0:d429f13fe4be 13 Ultrasonic F_sonic_R(p11,p11);
MR_Kang 0:d429f13fe4be 14 Ultrasonic F_sonic_L(p12,p12);
MR_Kang 0:d429f13fe4be 15 Ultrasonic F_sonic_F(p8,p8);
MR_Kang 0:d429f13fe4be 16
MR_Kang 0:d429f13fe4be 17 PwmOut RMotor_Front(p21);
MR_Kang 0:d429f13fe4be 18 PwmOut LMotor_Front(p23);
MR_Kang 0:d429f13fe4be 19 PwmOut RMotor_Back(p22);
MR_Kang 0:d429f13fe4be 20 PwmOut LMotor_Back(p24);
MR_Kang 0:d429f13fe4be 21 DigitalOut RMotor_EN(p5);
MR_Kang 0:d429f13fe4be 22 DigitalOut LMotor_EN(p6);
leejong87 2:c9740fccf3a7 23
leejong87 2:c9740fccf3a7 24 Ticker control;
MR_Kang 3:59a400d203cc 25 Timeout casemeasure;
MR_Kang 0:d429f13fe4be 26 Timeout motortime;
MR_Kang 0:d429f13fe4be 27 Timeout backmotortime;
MR_Kang 0:d429f13fe4be 28 Timeout Rightmotortime;
MR_Kang 0:d429f13fe4be 29 Timeout Leftmotortime;
leejong87 2:c9740fccf3a7 30
MR_Kang 0:d429f13fe4be 31 float a,b =0;
MR_Kang 0:d429f13fe4be 32 float length;
leejong87 2:c9740fccf3a7 33 int dis_L,dis_R,dis_F = 80;
leejong87 2:c9740fccf3a7 34 int F_len;
MR_Kang 3:59a400d203cc 35 int move,measure=1,flag=1,clag =1;
leejong87 2:c9740fccf3a7 36 int rssi1[5],rssi2[5],rssi3[5];
leejong87 2:c9740fccf3a7 37
leejong87 2:c9740fccf3a7 38 double d1, d2, d3, alpha, angle, distance; //정면, 좌측, 우측, 각도, 최종 각도, 최종 거리
leejong87 2:c9740fccf3a7 39 double rssi1_sum,rssi2_sum,rssi3_sum,rssi1_avg,rssi2_avg,rssi3_avg;
leejong87 2:c9740fccf3a7 40
leejong87 2:c9740fccf3a7 41 char xbeechar1, xbeebuf1[3], xbeechar2, xbeebuf2[3] ,xbeechar3, xbeebuf3[3];
leejong87 2:c9740fccf3a7 42 int xbeeflag1=0, xbeecount1=0, xbeeflag2=0, xbeecount2=0, xbeeflag3=0, xbeecount3=0;
leejong87 2:c9740fccf3a7 43
leejong87 2:c9740fccf3a7 44 char *stop;
leejong87 2:c9740fccf3a7 45
leejong87 2:c9740fccf3a7 46 void callback1()
leejong87 2:c9740fccf3a7 47 {
leejong87 2:c9740fccf3a7 48 xbeechar1 = xbee1.getc();
leejong87 2:c9740fccf3a7 49 if(xbeechar1 == '\r') {
leejong87 2:c9740fccf3a7 50 //pc.printf("\n");
leejong87 2:c9740fccf3a7 51 } else if ((xbeechar1 != 'T')&&(xbeechar1 != 'O')&&(xbeechar1 != 'K')) {
leejong87 2:c9740fccf3a7 52 //pc.printf("%c", xbeechar1);
leejong87 2:c9740fccf3a7 53 }
leejong87 2:c9740fccf3a7 54 if(xbeeflag1 == 1) {
leejong87 2:c9740fccf3a7 55 xbeebuf1[xbeecount1++]=xbeechar1;
leejong87 2:c9740fccf3a7 56 }
leejong87 2:c9740fccf3a7 57 }
leejong87 2:c9740fccf3a7 58 void callback2()
leejong87 2:c9740fccf3a7 59 {
leejong87 2:c9740fccf3a7 60 xbeechar2 = xbee2.getc();
leejong87 2:c9740fccf3a7 61 if(xbeechar2 == '\r') {
leejong87 2:c9740fccf3a7 62 //pc.printf("\n");
leejong87 2:c9740fccf3a7 63
leejong87 2:c9740fccf3a7 64 } else if ((xbeechar2 != 'T')&&(xbeechar2 != 'O')&&(xbeechar2 != 'K')) {
leejong87 2:c9740fccf3a7 65 //pc.printf("%c", xbeechar2);
leejong87 2:c9740fccf3a7 66 }
leejong87 2:c9740fccf3a7 67 if(xbeeflag2 == 1) {
leejong87 2:c9740fccf3a7 68 xbeebuf2[xbeecount2++]=xbeechar2;
leejong87 2:c9740fccf3a7 69 }
leejong87 2:c9740fccf3a7 70 }
leejong87 2:c9740fccf3a7 71 void callback3()
leejong87 2:c9740fccf3a7 72 {
leejong87 2:c9740fccf3a7 73 xbeechar3 = xbee3.getc();
leejong87 2:c9740fccf3a7 74 if(xbeechar3 == '\r') {
leejong87 2:c9740fccf3a7 75 //pc.printf("\n");
leejong87 2:c9740fccf3a7 76
leejong87 2:c9740fccf3a7 77 } else if ((xbeechar3 != 'T')&&(xbeechar3 != 'O')&&(xbeechar3 != 'K')) {
leejong87 2:c9740fccf3a7 78 //pc.printf("%c", xbeechar3);
leejong87 2:c9740fccf3a7 79 }
leejong87 2:c9740fccf3a7 80 if(xbeeflag3 == 1) {
leejong87 2:c9740fccf3a7 81 xbeebuf3[xbeecount3++]=xbeechar3;
leejong87 2:c9740fccf3a7 82 }
leejong87 2:c9740fccf3a7 83 }
leejong87 2:c9740fccf3a7 84
MR_Kang 0:d429f13fe4be 85 void Front()
MR_Kang 0:d429f13fe4be 86 {
MR_Kang 0:d429f13fe4be 87 RMotor_EN = 1;
MR_Kang 0:d429f13fe4be 88 LMotor_EN = 1;
MR_Kang 0:d429f13fe4be 89 RMotor_Back.pulsewidth(0);
MR_Kang 0:d429f13fe4be 90 LMotor_Back.pulsewidth(0);
MR_Kang 0:d429f13fe4be 91 RMotor_Front.pulsewidth(0.05);
MR_Kang 0:d429f13fe4be 92 LMotor_Front.pulsewidth(0.05);
MR_Kang 0:d429f13fe4be 93 }
MR_Kang 0:d429f13fe4be 94
MR_Kang 0:d429f13fe4be 95 void Turn_R()
MR_Kang 0:d429f13fe4be 96 {
MR_Kang 0:d429f13fe4be 97 RMotor_EN = 1;
MR_Kang 0:d429f13fe4be 98 LMotor_EN =1;
MR_Kang 0:d429f13fe4be 99 LMotor_Front.pulsewidth(0);
MR_Kang 0:d429f13fe4be 100 RMotor_Back.pulsewidth(0);
MR_Kang 0:d429f13fe4be 101 LMotor_Back.pulsewidth(0.1);
MR_Kang 0:d429f13fe4be 102 RMotor_Front.pulsewidth(0.1);
MR_Kang 0:d429f13fe4be 103 }
MR_Kang 0:d429f13fe4be 104
MR_Kang 0:d429f13fe4be 105 void Turn_L()
MR_Kang 0:d429f13fe4be 106 {
MR_Kang 0:d429f13fe4be 107 LMotor_EN = 1;
MR_Kang 0:d429f13fe4be 108 RMotor_EN = 1;
MR_Kang 0:d429f13fe4be 109 LMotor_Back.pulsewidth(0);
MR_Kang 0:d429f13fe4be 110 RMotor_Front.pulsewidth(0);
MR_Kang 0:d429f13fe4be 111 RMotor_Back.pulsewidth(0.1);
MR_Kang 0:d429f13fe4be 112 LMotor_Front.pulsewidth(0.1);
MR_Kang 0:d429f13fe4be 113 }
MR_Kang 0:d429f13fe4be 114 void Back()
MR_Kang 0:d429f13fe4be 115 {
MR_Kang 0:d429f13fe4be 116 RMotor_EN = 1;
MR_Kang 0:d429f13fe4be 117 LMotor_EN = 1;
MR_Kang 0:d429f13fe4be 118 RMotor_Front.pulsewidth(0);
MR_Kang 0:d429f13fe4be 119 LMotor_Front.pulsewidth(0);
MR_Kang 0:d429f13fe4be 120 RMotor_Back.pulsewidth(0.05);
MR_Kang 0:d429f13fe4be 121 LMotor_Back.pulsewidth(0.05);
MR_Kang 0:d429f13fe4be 122 }
MR_Kang 0:d429f13fe4be 123 void Break()
MR_Kang 0:d429f13fe4be 124 {
MR_Kang 0:d429f13fe4be 125 RMotor_Front.pulsewidth(0);
MR_Kang 0:d429f13fe4be 126 LMotor_Front.pulsewidth(0);
MR_Kang 0:d429f13fe4be 127 RMotor_Back.pulsewidth(0.05);
MR_Kang 0:d429f13fe4be 128 LMotor_Back.pulsewidth(0.05);
MR_Kang 0:d429f13fe4be 129 wait(0.1);
MR_Kang 0:d429f13fe4be 130 RMotor_EN = 0;
MR_Kang 0:d429f13fe4be 131 LMotor_EN = 0;
MR_Kang 0:d429f13fe4be 132 }
MR_Kang 0:d429f13fe4be 133 void Stop()
MR_Kang 0:d429f13fe4be 134 {
MR_Kang 0:d429f13fe4be 135 RMotor_EN = 0;
MR_Kang 0:d429f13fe4be 136 LMotor_EN = 0;
MR_Kang 0:d429f13fe4be 137 RMotor_Front.pulsewidth(0);
MR_Kang 0:d429f13fe4be 138 LMotor_Front.pulsewidth(0);
MR_Kang 0:d429f13fe4be 139 RMotor_Back.pulsewidth(0);
MR_Kang 0:d429f13fe4be 140 LMotor_Back.pulsewidth(0);
MR_Kang 0:d429f13fe4be 141 }
MR_Kang 0:d429f13fe4be 142
MR_Kang 0:d429f13fe4be 143 void count()
MR_Kang 0:d429f13fe4be 144 {
MR_Kang 0:d429f13fe4be 145 dis_F =100;
MR_Kang 0:d429f13fe4be 146 dis_L =100;
MR_Kang 0:d429f13fe4be 147 dis_R =100;
MR_Kang 3:59a400d203cc 148 // measure =0;
MR_Kang 0:d429f13fe4be 149 flag=1;
MR_Kang 0:d429f13fe4be 150 }
MR_Kang 0:d429f13fe4be 151
leejong87 2:c9740fccf3a7 152 void backcount()
leejong87 2:c9740fccf3a7 153 {
leejong87 2:c9740fccf3a7 154 move = 1;
MR_Kang 3:59a400d203cc 155 // measure =0;
leejong87 2:c9740fccf3a7 156 }
leejong87 2:c9740fccf3a7 157 void Rightcount()
leejong87 2:c9740fccf3a7 158 {
leejong87 2:c9740fccf3a7 159 move = 2;
MR_Kang 3:59a400d203cc 160 // measure =0;
leejong87 2:c9740fccf3a7 161 }
leejong87 2:c9740fccf3a7 162 void Leftcount()
leejong87 2:c9740fccf3a7 163 {
leejong87 2:c9740fccf3a7 164 move = 3;
MR_Kang 3:59a400d203cc 165 // measure =0;
leejong87 2:c9740fccf3a7 166 }
leejong87 2:c9740fccf3a7 167
MR_Kang 3:59a400d203cc 168 void measure_case1()
MR_Kang 3:59a400d203cc 169 {
MR_Kang 3:59a400d203cc 170 clag =1;
MR_Kang 3:59a400d203cc 171 move =1;
MR_Kang 3:59a400d203cc 172 //measure =1;
MR_Kang 3:59a400d203cc 173 }
MR_Kang 3:59a400d203cc 174 void measure_case0()
MR_Kang 3:59a400d203cc 175 {
MR_Kang 3:59a400d203cc 176 //measure =0;
MR_Kang 3:59a400d203cc 177 clag =1;
MR_Kang 3:59a400d203cc 178 }
leejong87 2:c9740fccf3a7 179 void control_func()
leejong87 2:c9740fccf3a7 180 {
leejong87 2:c9740fccf3a7 181
leejong87 2:c9740fccf3a7 182 //거리측정
leejong87 2:c9740fccf3a7 183 if(dis_F==F_sonic_F.read()/10) {
leejong87 2:c9740fccf3a7 184 dis_F = 80;
leejong87 2:c9740fccf3a7 185 }
leejong87 2:c9740fccf3a7 186 if(dis_R==F_sonic_R.read()/10) {
leejong87 2:c9740fccf3a7 187 dis_R = 90;
leejong87 2:c9740fccf3a7 188 }
leejong87 2:c9740fccf3a7 189 if(dis_L==F_sonic_L.read()/10) {
leejong87 2:c9740fccf3a7 190 dis_L = 90;
leejong87 2:c9740fccf3a7 191 }
leejong87 2:c9740fccf3a7 192
leejong87 2:c9740fccf3a7 193 if(dis_F!=F_sonic_F.read()/10) {
leejong87 2:c9740fccf3a7 194 dis_F = F_sonic_F.read()/10;
leejong87 2:c9740fccf3a7 195 }
leejong87 2:c9740fccf3a7 196 if(dis_R!=F_sonic_R.read()/10) {
leejong87 2:c9740fccf3a7 197 dis_R = F_sonic_R.read()/10;
leejong87 2:c9740fccf3a7 198 }
leejong87 2:c9740fccf3a7 199 if(dis_L!=F_sonic_L.read()/10) {
leejong87 2:c9740fccf3a7 200 dis_L = F_sonic_L.read()/10;
MR_Kang 0:d429f13fe4be 201 }
leejong87 2:c9740fccf3a7 202
leejong87 2:c9740fccf3a7 203 /* if(dis_R <50) {
leejong87 2:c9740fccf3a7 204 if(dis_L <50){
leejong87 2:c9740fccf3a7 205 if(dis_F >50){
leejong87 2:c9740fccf3a7 206 move =0;}}}
leejong87 2:c9740fccf3a7 207 else if(dis_L <50) {
leejong87 2:c9740fccf3a7 208 if(dis_R <50){
leejong87 2:c9740fccf3a7 209 if(dt is_F >50){
leejong87 2:c9740fccf3a7 210 move =0;}}}*/
leejong87 2:c9740fccf3a7 211
leejong87 2:c9740fccf3a7 212
leejong87 2:c9740fccf3a7 213 switch(measure) {
leejong87 2:c9740fccf3a7 214 case 0:
leejong87 2:c9740fccf3a7 215 if(dis_F >30) {
leejong87 2:c9740fccf3a7 216 if(dis_R <50) {
leejong87 2:c9740fccf3a7 217 if((dis_R + 5) < dis_L) {
leejong87 2:c9740fccf3a7 218 move =2;
leejong87 2:c9740fccf3a7 219 }
leejong87 2:c9740fccf3a7 220 } else if(dis_L <50) {
leejong87 2:c9740fccf3a7 221 if((dis_L + 5) < dis_R) {
leejong87 2:c9740fccf3a7 222 move =3;
leejong87 2:c9740fccf3a7 223 }
leejong87 2:c9740fccf3a7 224 } else {
leejong87 2:c9740fccf3a7 225 move =0;
leejong87 2:c9740fccf3a7 226 }
leejong87 2:c9740fccf3a7 227 } else if(dis_F<30) {
leejong87 2:c9740fccf3a7 228 move =1;
MR_Kang 3:59a400d203cc 229 }
MR_Kang 3:59a400d203cc 230 if(dis_R <10) {
MR_Kang 3:59a400d203cc 231 move=5;
MR_Kang 3:59a400d203cc 232 } else if(dis_L <10) {
MR_Kang 3:59a400d203cc 233 move =6;
MR_Kang 3:59a400d203cc 234 } else if((dis_R <25) &&(dis_L<25)) {
MR_Kang 3:59a400d203cc 235 move =4;
MR_Kang 3:59a400d203cc 236 } else if(dis_R <35) {
MR_Kang 3:59a400d203cc 237 move =2;
MR_Kang 3:59a400d203cc 238 } else if(dis_L <35) {
MR_Kang 3:59a400d203cc 239 move =3;
MR_Kang 3:59a400d203cc 240 }
MR_Kang 3:59a400d203cc 241 if(clag ==1) {
MR_Kang 3:59a400d203cc 242 clag =0;
MR_Kang 3:59a400d203cc 243 casemeasure.attach(&measure_case1,3.0);
leejong87 2:c9740fccf3a7 244 }
leejong87 2:c9740fccf3a7 245 break;
MR_Kang 3:59a400d203cc 246
MR_Kang 3:59a400d203cc 247 case 1:
MR_Kang 3:59a400d203cc 248 if((distance >500.0) && ((angle*180/3.14) < 10.0) && ((angle*180/3.14) > -10.0)) {
MR_Kang 3:59a400d203cc 249 move = 1;// measure =0; flag =0;
MR_Kang 3:59a400d203cc 250 } else if((distance >500.0) && ((angle*180/3.14) > 10.0)) {
MR_Kang 3:59a400d203cc 251 move = 2;
MR_Kang 3:59a400d203cc 252 } else if((distance >500.0) && ((angle*180/3.14) < -10.0)) {
MR_Kang 3:59a400d203cc 253 move = 3;
MR_Kang 3:59a400d203cc 254 }
MR_Kang 3:59a400d203cc 255 /* if(clag ==1){
MR_Kang 3:59a400d203cc 256 if(distance >500.0) {
MR_Kang 3:59a400d203cc 257 clag =0;
MR_Kang 3:59a400d203cc 258 casemeasure.attach(&measure_case0,1);
MR_Kang 3:59a400d203cc 259 }
MR_Kang 3:59a400d203cc 260 }*/
MR_Kang 3:59a400d203cc 261 break;
MR_Kang 0:d429f13fe4be 262 }
leejong87 2:c9740fccf3a7 263 switch(move) {
leejong87 2:c9740fccf3a7 264 case 0://Front
leejong87 2:c9740fccf3a7 265 Front();
leejong87 2:c9740fccf3a7 266 break;
leejong87 2:c9740fccf3a7 267
leejong87 2:c9740fccf3a7 268 case 1://Back
leejong87 2:c9740fccf3a7 269 Stop();
leejong87 2:c9740fccf3a7 270 break;
leejong87 2:c9740fccf3a7 271
leejong87 2:c9740fccf3a7 272 case 2://Right
leejong87 2:c9740fccf3a7 273 Turn_R();
leejong87 2:c9740fccf3a7 274 if (flag==1) {
leejong87 2:c9740fccf3a7 275 flag=0;
leejong87 2:c9740fccf3a7 276 motortime.attach(&count,1);
leejong87 2:c9740fccf3a7 277 }
leejong87 2:c9740fccf3a7 278 break;
leejong87 2:c9740fccf3a7 279
leejong87 2:c9740fccf3a7 280 case 3://Left
leejong87 2:c9740fccf3a7 281 Turn_L();
leejong87 2:c9740fccf3a7 282 if (flag==1) {
leejong87 2:c9740fccf3a7 283 flag=0;
leejong87 2:c9740fccf3a7 284 motortime.attach(&count,1);
leejong87 2:c9740fccf3a7 285 }
leejong87 2:c9740fccf3a7 286 break;
leejong87 2:c9740fccf3a7 287
leejong87 2:c9740fccf3a7 288 case 4://back
leejong87 2:c9740fccf3a7 289 Back();
leejong87 2:c9740fccf3a7 290 backmotortime.attach(&backcount,2);
leejong87 2:c9740fccf3a7 291 break;
leejong87 2:c9740fccf3a7 292 case 5://Right
leejong87 2:c9740fccf3a7 293 Back();
leejong87 2:c9740fccf3a7 294 Rightmotortime.attach(&Rightcount,2);
leejong87 2:c9740fccf3a7 295 break;
leejong87 2:c9740fccf3a7 296 case 6://back
leejong87 2:c9740fccf3a7 297 Back();
leejong87 2:c9740fccf3a7 298 Leftmotortime.attach(&Leftcount,2);
leejong87 2:c9740fccf3a7 299 break;
leejong87 2:c9740fccf3a7 300 }
leejong87 2:c9740fccf3a7 301 }
leejong87 2:c9740fccf3a7 302
MR_Kang 0:d429f13fe4be 303 int main()
MR_Kang 0:d429f13fe4be 304 {
MR_Kang 0:d429f13fe4be 305
leejong87 2:c9740fccf3a7 306 int Gc=0;
MR_Kang 0:d429f13fe4be 307
leejong87 2:c9740fccf3a7 308 control.attach(&control_func,0.1);
leejong87 2:c9740fccf3a7 309 xbee1.attach(&callback1);
leejong87 2:c9740fccf3a7 310 xbee2.attach(&callback2);
leejong87 2:c9740fccf3a7 311 xbee3.attach(&callback3);
MR_Kang 0:d429f13fe4be 312
leejong87 2:c9740fccf3a7 313 while(1) {
leejong87 2:c9740fccf3a7 314 xbee1.printf("+++");
MR_Kang 3:59a400d203cc 315 wait(0.05);
leejong87 2:c9740fccf3a7 316 xbee2.printf("+++");
MR_Kang 3:59a400d203cc 317 wait(0.05);
leejong87 2:c9740fccf3a7 318 xbee3.printf("+++");
leejong87 2:c9740fccf3a7 319 wait(1);
leejong87 2:c9740fccf3a7 320 xbeecount1=0;
leejong87 2:c9740fccf3a7 321 xbeeflag1 = 1;
leejong87 2:c9740fccf3a7 322 xbee1.printf("ATDB\r");
MR_Kang 3:59a400d203cc 323 wait(0.05);
leejong87 2:c9740fccf3a7 324 xbeecount2=0;
leejong87 2:c9740fccf3a7 325 xbeeflag2 = 1;
leejong87 2:c9740fccf3a7 326 xbee2.printf("ATDB\r");
MR_Kang 3:59a400d203cc 327 wait(0.05);
leejong87 2:c9740fccf3a7 328 xbeecount3=0;
leejong87 2:c9740fccf3a7 329 xbeeflag3 = 1;
leejong87 2:c9740fccf3a7 330 xbee3.printf("ATDB\r");
leejong87 2:c9740fccf3a7 331 wait(1);
leejong87 2:c9740fccf3a7 332 xbeeflag1 = 0;
leejong87 2:c9740fccf3a7 333 xbeeflag2 = 0;
leejong87 2:c9740fccf3a7 334 xbeeflag3 = 0;
leejong87 2:c9740fccf3a7 335 rssi1[Gc] = strtol( xbeebuf1, &stop , 16 );
leejong87 2:c9740fccf3a7 336 rssi2[Gc] = strtol( xbeebuf2, &stop , 16 );
leejong87 2:c9740fccf3a7 337 rssi3[Gc] = strtol( xbeebuf3, &stop , 16 );
leejong87 2:c9740fccf3a7 338 xbee1.printf("ATCN\r");
MR_Kang 3:59a400d203cc 339 wait(0.05);
leejong87 2:c9740fccf3a7 340 xbee2.printf("ATCN\r");
MR_Kang 3:59a400d203cc 341 wait(0.05);
leejong87 2:c9740fccf3a7 342 xbee3.printf("ATCN\r");
leejong87 2:c9740fccf3a7 343 wait(1);
leejong87 2:c9740fccf3a7 344
leejong87 2:c9740fccf3a7 345 rssi1_sum = 0;
leejong87 2:c9740fccf3a7 346 rssi2_sum = 0;
leejong87 2:c9740fccf3a7 347 rssi3_sum = 0;
leejong87 2:c9740fccf3a7 348
MR_Kang 3:59a400d203cc 349 for (int i=0; i<3; i++) {
leejong87 2:c9740fccf3a7 350 rssi1_sum += rssi1[i];
leejong87 2:c9740fccf3a7 351 rssi2_sum += rssi2[i];
leejong87 2:c9740fccf3a7 352 rssi3_sum += rssi3[i];
leejong87 2:c9740fccf3a7 353 }
MR_Kang 3:59a400d203cc 354 rssi1_avg = rssi1_sum / 3.0;
MR_Kang 3:59a400d203cc 355 rssi2_avg = rssi2_sum / 3.0;
MR_Kang 3:59a400d203cc 356 rssi3_avg = rssi3_sum / 3.0;
leejong87 2:c9740fccf3a7 357
leejong87 2:c9740fccf3a7 358
leejong87 2:c9740fccf3a7 359 printf("RSSI:%ddBm,%ddBm,%ddBm\n",rssi1[Gc],rssi2[Gc],rssi3[Gc]);
MR_Kang 3:59a400d203cc 360 bt.printf("RSSI:%ddBm,%ddBm,%ddBm\n",rssi1[Gc],rssi2[Gc],rssi3[Gc]);
leejong87 2:c9740fccf3a7 361 printf("AVG :%fdBm,%fdBm,%fdBm\n",rssi1_avg,rssi2_avg,rssi3_avg);
leejong87 2:c9740fccf3a7 362
leejong87 2:c9740fccf3a7 363 d1 = rssi1_avg*20;
leejong87 2:c9740fccf3a7 364 d2 = rssi2_avg*20;
leejong87 2:c9740fccf3a7 365 d3 = rssi3_avg*20;
leejong87 2:c9740fccf3a7 366
leejong87 2:c9740fccf3a7 367 printf("DIST:%f, %f, %f\n\n",d1,d2,d3);
leejong87 2:c9740fccf3a7 368
leejong87 2:c9740fccf3a7 369 if (d2 == d3) { //center
leejong87 2:c9740fccf3a7 370 if(d1 <= d3) { //front
leejong87 2:c9740fccf3a7 371 alpha = acos( (d3*d3+400.0*400.0-d2*d2)/(2*d3*400.0) );
leejong87 2:c9740fccf3a7 372 angle = 0;
leejong87 2:c9740fccf3a7 373 distance = d3 * sin(alpha) / sin(1.57-angle);
leejong87 2:c9740fccf3a7 374 } else if (d1 > d3) { //rear
leejong87 2:c9740fccf3a7 375 alpha = acos( (d3*d3+400.0*400.0-d2*d2)/(2*d3*400.0) );
leejong87 2:c9740fccf3a7 376 angle = 3.14;
leejong87 2:c9740fccf3a7 377 distance = d3 * sin(alpha) / sin(1.57-angle);
MR_Kang 0:d429f13fe4be 378 }
leejong87 2:c9740fccf3a7 379 } else if(d2 < d3) { //left
leejong87 2:c9740fccf3a7 380 if(d1 <= d3) { //front
leejong87 2:c9740fccf3a7 381 alpha = acos( (d3*d3+400.0*400.0-d2*d2)/(2*d3*400.0) );
leejong87 2:c9740fccf3a7 382 angle = 1.57 - atan( (d3 * sin(alpha))/(d3 * cos(alpha) - 200.0)); // +0 ~ +90
leejong87 2:c9740fccf3a7 383 distance = d3 * sin(alpha) / sin(1.57-angle);
leejong87 2:c9740fccf3a7 384 } else if (d1 > d3) { //rear
leejong87 2:c9740fccf3a7 385 alpha = acos( (d3*d3+400.0*400.0-d2*d2)/(2*d3*400.0) );
leejong87 2:c9740fccf3a7 386 angle = 1.57 + atan( (d3 * sin(alpha))/(d3 * cos(alpha) - 200.0)); // // +90 ~ +180
leejong87 2:c9740fccf3a7 387 distance = d3 * sin(alpha) / sin(1.57-angle);
MR_Kang 0:d429f13fe4be 388 }
MR_Kang 0:d429f13fe4be 389
leejong87 2:c9740fccf3a7 390 } else if(d2 > d3) { //right
leejong87 2:c9740fccf3a7 391 if(d1 <= d2) { //front
leejong87 2:c9740fccf3a7 392 alpha = acos( (d2*d2+400.0*400.0-d3*d3)/(2*d2*400.0) );
leejong87 2:c9740fccf3a7 393 angle = -1.57 + atan( (d2 * sin(alpha))/(d2 * cos(alpha) - 200.0)) ; // -0 ~ -90
leejong87 2:c9740fccf3a7 394 distance = d2 * sin(alpha) / sin(1.57+angle);
leejong87 2:c9740fccf3a7 395 } else if (d1 > d2) { //rear
leejong87 2:c9740fccf3a7 396 alpha = acos( (d2*d2+400.0*400.0-d3*d3)/(2*d2*400.0) );
leejong87 2:c9740fccf3a7 397 angle = -1.57 - atan( (d2 * sin(alpha))/(d2 * cos(alpha) - 200.0)); // -90 ~ -180
leejong87 2:c9740fccf3a7 398 distance = d2 * sin(alpha) / sin(1.57+angle);
leejong87 2:c9740fccf3a7 399 }
leejong87 2:c9740fccf3a7 400 }
MR_Kang 0:d429f13fe4be 401
MR_Kang 3:59a400d203cc 402 printf("alpha : %f, angle : %f, distance : %f move = %d %d\n\n",alpha*180/3.14,angle*180/3.14,distance,move,measure);//++ left
MR_Kang 3:59a400d203cc 403 bt.printf("alpha : %f, angle : %f, distance : %f move = %d %d\n\n",alpha*180/3.14,angle*180/3.14,distance,move,measure);//++ left
MR_Kang 3:59a400d203cc 404 if(Gc<2) {
MR_Kang 3:59a400d203cc 405 Gc++;
MR_Kang 3:59a400d203cc 406 } else {
leejong87 2:c9740fccf3a7 407 Gc=0;
MR_Kang 0:d429f13fe4be 408 }
MR_Kang 0:d429f13fe4be 409 }
MR_Kang 0:d429f13fe4be 410
MR_Kang 0:d429f13fe4be 411 }