![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |