Kang hokyoung / Mbed 2 deprecated 2014_Ensoul_Capstone

2014_Ensoul_Capstone

Dependencies:   TextLCD Ultrasonic mbed BufferedSoftSerial

main.cpp@2:c9740fccf3a7, 2014-07-03 (annotated)

Committer:
leejong87
Date:
Thu Jul 03 07:34:06 2014 +0000
Revision:
2:c9740fccf3a7
Parent:
1:2859bfed20b4
Child:
3:59a400d203cc
Add RSSI

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