Kang hokyoung / Mbed 2 deprecated 2014_Ensoul_Capstone

2014_Ensoul_Capstone

Dependencies:   TextLCD Ultrasonic mbed BufferedSoftSerial

main.cpp@5:58e37c4502ea, 2014-07-08 (annotated)

Committer:
leejong87
Date:
Tue Jul 08 09:40:12 2014 +0000
Revision:
5:58e37c4502ea
Parent:
4:fd36ff807a91 
2014/07/08

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