![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2014_Ensoul_Capstone
Dependencies: TextLCD Ultrasonic mbed BufferedSoftSerial
main.cpp@0:d429f13fe4be, 2014-07-03 (annotated)
- Committer:
- MR_Kang
- Date:
- Thu Jul 03 05:13:08 2014 +0000
- Revision:
- 0:d429f13fe4be
- Child:
- 1:2859bfed20b4
Publish
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MR_Kang | 0:d429f13fe4be | 1 | #include "mbed.h" |
MR_Kang | 0:d429f13fe4be | 2 | #include "Ultrasonic.h" |
MR_Kang | 0:d429f13fe4be | 3 | |
MR_Kang | 0:d429f13fe4be | 4 | AnalogIn Sharp(p20); |
MR_Kang | 0:d429f13fe4be | 5 | Ultrasonic F_sonic_R(p11,p11); |
MR_Kang | 0:d429f13fe4be | 6 | Ultrasonic F_sonic_L(p12,p12); |
MR_Kang | 0:d429f13fe4be | 7 | Ultrasonic F_sonic_F(p8,p8); |
MR_Kang | 0:d429f13fe4be | 8 | |
MR_Kang | 0:d429f13fe4be | 9 | PwmOut RMotor_Front(p21); |
MR_Kang | 0:d429f13fe4be | 10 | PwmOut LMotor_Front(p23); |
MR_Kang | 0:d429f13fe4be | 11 | PwmOut RMotor_Back(p22); |
MR_Kang | 0:d429f13fe4be | 12 | PwmOut LMotor_Back(p24); |
MR_Kang | 0:d429f13fe4be | 13 | DigitalOut RMotor_EN(p5); |
MR_Kang | 0:d429f13fe4be | 14 | DigitalOut LMotor_EN(p6); |
MR_Kang | 0:d429f13fe4be | 15 | Timeout motortime; |
MR_Kang | 0:d429f13fe4be | 16 | Timeout backmotortime; |
MR_Kang | 0:d429f13fe4be | 17 | Timeout Rightmotortime; |
MR_Kang | 0:d429f13fe4be | 18 | Timeout Leftmotortime; |
MR_Kang | 0:d429f13fe4be | 19 | float a,b =0; |
MR_Kang | 0:d429f13fe4be | 20 | float length; |
MR_Kang | 0:d429f13fe4be | 21 | void Front() |
MR_Kang | 0:d429f13fe4be | 22 | { |
MR_Kang | 0:d429f13fe4be | 23 | RMotor_EN = 1; |
MR_Kang | 0:d429f13fe4be | 24 | LMotor_EN = 1; |
MR_Kang | 0:d429f13fe4be | 25 | RMotor_Back.pulsewidth(0); |
MR_Kang | 0:d429f13fe4be | 26 | LMotor_Back.pulsewidth(0); |
MR_Kang | 0:d429f13fe4be | 27 | RMotor_Front.pulsewidth(0.05); |
MR_Kang | 0:d429f13fe4be | 28 | LMotor_Front.pulsewidth(0.05); |
MR_Kang | 0:d429f13fe4be | 29 | } |
MR_Kang | 0:d429f13fe4be | 30 | |
MR_Kang | 0:d429f13fe4be | 31 | void Turn_R() |
MR_Kang | 0:d429f13fe4be | 32 | { |
MR_Kang | 0:d429f13fe4be | 33 | RMotor_EN = 1; |
MR_Kang | 0:d429f13fe4be | 34 | LMotor_EN =1; |
MR_Kang | 0:d429f13fe4be | 35 | LMotor_Front.pulsewidth(0); |
MR_Kang | 0:d429f13fe4be | 36 | RMotor_Back.pulsewidth(0); |
MR_Kang | 0:d429f13fe4be | 37 | LMotor_Back.pulsewidth(0.1); |
MR_Kang | 0:d429f13fe4be | 38 | RMotor_Front.pulsewidth(0.1); |
MR_Kang | 0:d429f13fe4be | 39 | } |
MR_Kang | 0:d429f13fe4be | 40 | |
MR_Kang | 0:d429f13fe4be | 41 | void Turn_L() |
MR_Kang | 0:d429f13fe4be | 42 | { |
MR_Kang | 0:d429f13fe4be | 43 | LMotor_EN = 1; |
MR_Kang | 0:d429f13fe4be | 44 | RMotor_EN = 1; |
MR_Kang | 0:d429f13fe4be | 45 | LMotor_Back.pulsewidth(0); |
MR_Kang | 0:d429f13fe4be | 46 | RMotor_Front.pulsewidth(0); |
MR_Kang | 0:d429f13fe4be | 47 | RMotor_Back.pulsewidth(0.1); |
MR_Kang | 0:d429f13fe4be | 48 | LMotor_Front.pulsewidth(0.1); |
MR_Kang | 0:d429f13fe4be | 49 | } |
MR_Kang | 0:d429f13fe4be | 50 | void Back() |
MR_Kang | 0:d429f13fe4be | 51 | { |
MR_Kang | 0:d429f13fe4be | 52 | RMotor_EN = 1; |
MR_Kang | 0:d429f13fe4be | 53 | LMotor_EN = 1; |
MR_Kang | 0:d429f13fe4be | 54 | RMotor_Front.pulsewidth(0); |
MR_Kang | 0:d429f13fe4be | 55 | LMotor_Front.pulsewidth(0); |
MR_Kang | 0:d429f13fe4be | 56 | RMotor_Back.pulsewidth(0.05); |
MR_Kang | 0:d429f13fe4be | 57 | LMotor_Back.pulsewidth(0.05); |
MR_Kang | 0:d429f13fe4be | 58 | } |
MR_Kang | 0:d429f13fe4be | 59 | void Break() |
MR_Kang | 0:d429f13fe4be | 60 | { |
MR_Kang | 0:d429f13fe4be | 61 | RMotor_Front.pulsewidth(0); |
MR_Kang | 0:d429f13fe4be | 62 | LMotor_Front.pulsewidth(0); |
MR_Kang | 0:d429f13fe4be | 63 | RMotor_Back.pulsewidth(0.05); |
MR_Kang | 0:d429f13fe4be | 64 | LMotor_Back.pulsewidth(0.05); |
MR_Kang | 0:d429f13fe4be | 65 | wait(0.1); |
MR_Kang | 0:d429f13fe4be | 66 | RMotor_EN = 0; |
MR_Kang | 0:d429f13fe4be | 67 | LMotor_EN = 0; |
MR_Kang | 0:d429f13fe4be | 68 | } |
MR_Kang | 0:d429f13fe4be | 69 | void Stop() |
MR_Kang | 0:d429f13fe4be | 70 | { |
MR_Kang | 0:d429f13fe4be | 71 | RMotor_EN = 0; |
MR_Kang | 0:d429f13fe4be | 72 | LMotor_EN = 0; |
MR_Kang | 0:d429f13fe4be | 73 | RMotor_Front.pulsewidth(0); |
MR_Kang | 0:d429f13fe4be | 74 | LMotor_Front.pulsewidth(0); |
MR_Kang | 0:d429f13fe4be | 75 | RMotor_Back.pulsewidth(0); |
MR_Kang | 0:d429f13fe4be | 76 | LMotor_Back.pulsewidth(0); |
MR_Kang | 0:d429f13fe4be | 77 | } |
MR_Kang | 0:d429f13fe4be | 78 | |
MR_Kang | 0:d429f13fe4be | 79 | |
MR_Kang | 0:d429f13fe4be | 80 | int dis_L,dis_R,dis_F = 80; |
MR_Kang | 0:d429f13fe4be | 81 | int F_len; |
MR_Kang | 0:d429f13fe4be | 82 | int move,measure,flag=1; |
MR_Kang | 0:d429f13fe4be | 83 | |
MR_Kang | 0:d429f13fe4be | 84 | void count() |
MR_Kang | 0:d429f13fe4be | 85 | { |
MR_Kang | 0:d429f13fe4be | 86 | dis_F =100; |
MR_Kang | 0:d429f13fe4be | 87 | dis_L =100; |
MR_Kang | 0:d429f13fe4be | 88 | dis_R =100; |
MR_Kang | 0:d429f13fe4be | 89 | move =0; |
MR_Kang | 0:d429f13fe4be | 90 | measure =0; |
MR_Kang | 0:d429f13fe4be | 91 | flag=1; |
MR_Kang | 0:d429f13fe4be | 92 | } |
MR_Kang | 0:d429f13fe4be | 93 | |
MR_Kang | 0:d429f13fe4be | 94 | void backcount(){ |
MR_Kang | 0:d429f13fe4be | 95 | move = 1; measure =0; |
MR_Kang | 0:d429f13fe4be | 96 | } |
MR_Kang | 0:d429f13fe4be | 97 | void Rightcount(){ |
MR_Kang | 0:d429f13fe4be | 98 | move = 2; measure =0; |
MR_Kang | 0:d429f13fe4be | 99 | } |
MR_Kang | 0:d429f13fe4be | 100 | void Leftcount(){ |
MR_Kang | 0:d429f13fe4be | 101 | move = 3; measure =0; |
MR_Kang | 0:d429f13fe4be | 102 | } |
MR_Kang | 0:d429f13fe4be | 103 | int main() |
MR_Kang | 0:d429f13fe4be | 104 | { |
MR_Kang | 0:d429f13fe4be | 105 | |
MR_Kang | 0:d429f13fe4be | 106 | while(1) { |
MR_Kang | 0:d429f13fe4be | 107 | |
MR_Kang | 0:d429f13fe4be | 108 | //거리측정 |
MR_Kang | 0:d429f13fe4be | 109 | if(dis_F==F_sonic_F.read()/10) { |
MR_Kang | 0:d429f13fe4be | 110 | dis_F = 80;} |
MR_Kang | 0:d429f13fe4be | 111 | if(dis_R==F_sonic_R.read()/10) { |
MR_Kang | 0:d429f13fe4be | 112 | dis_R = 90;} |
MR_Kang | 0:d429f13fe4be | 113 | if(dis_L==F_sonic_L.read()/10) { |
MR_Kang | 0:d429f13fe4be | 114 | dis_L = 90;} |
MR_Kang | 0:d429f13fe4be | 115 | |
MR_Kang | 0:d429f13fe4be | 116 | if(dis_F!=F_sonic_F.read()/10) { |
MR_Kang | 0:d429f13fe4be | 117 | dis_F = F_sonic_F.read()/10; |
MR_Kang | 0:d429f13fe4be | 118 | } |
MR_Kang | 0:d429f13fe4be | 119 | if(dis_R!=F_sonic_R.read()/10) { |
MR_Kang | 0:d429f13fe4be | 120 | dis_R = F_sonic_R.read()/10; |
MR_Kang | 0:d429f13fe4be | 121 | } |
MR_Kang | 0:d429f13fe4be | 122 | if(dis_L!=F_sonic_L.read()/10) { |
MR_Kang | 0:d429f13fe4be | 123 | dis_L = F_sonic_L.read()/10; |
MR_Kang | 0:d429f13fe4be | 124 | } |
MR_Kang | 0:d429f13fe4be | 125 | |
MR_Kang | 0:d429f13fe4be | 126 | printf("Front: %d\n",dis_F); |
MR_Kang | 0:d429f13fe4be | 127 | printf("Right: %d\n",dis_L); |
MR_Kang | 0:d429f13fe4be | 128 | printf("Left : %d\n",dis_R); |
MR_Kang | 0:d429f13fe4be | 129 | printf("move : %d\n",move); |
MR_Kang | 0:d429f13fe4be | 130 | |
MR_Kang | 0:d429f13fe4be | 131 | /* if(dis_R <50) { |
MR_Kang | 0:d429f13fe4be | 132 | if(dis_L <50){ |
MR_Kang | 0:d429f13fe4be | 133 | if(dis_F >50){ |
MR_Kang | 0:d429f13fe4be | 134 | move =0;}}} |
MR_Kang | 0:d429f13fe4be | 135 | else if(dis_L <50) { |
MR_Kang | 0:d429f13fe4be | 136 | if(dis_R <50){ |
MR_Kang | 0:d429f13fe4be | 137 | if(dt is_F >50){ |
MR_Kang | 0:d429f13fe4be | 138 | move =0;}}}*/ |
MR_Kang | 0:d429f13fe4be | 139 | |
MR_Kang | 0:d429f13fe4be | 140 | |
MR_Kang | 0:d429f13fe4be | 141 | switch(measure){ |
MR_Kang | 0:d429f13fe4be | 142 | case 0: |
MR_Kang | 0:d429f13fe4be | 143 | if(dis_F >30) { |
MR_Kang | 0:d429f13fe4be | 144 | if(dis_R <50) { |
MR_Kang | 0:d429f13fe4be | 145 | if((dis_R + 5) < dis_L){ |
MR_Kang | 0:d429f13fe4be | 146 | move =2; measure =1;} |
MR_Kang | 0:d429f13fe4be | 147 | } else if(dis_L <50) { |
MR_Kang | 0:d429f13fe4be | 148 | if((dis_L + 5) < dis_R){ |
MR_Kang | 0:d429f13fe4be | 149 | move =3;measure =1;} |
MR_Kang | 0:d429f13fe4be | 150 | } else { |
MR_Kang | 0:d429f13fe4be | 151 | move =0; measure =1; |
MR_Kang | 0:d429f13fe4be | 152 | } |
MR_Kang | 0:d429f13fe4be | 153 | } else if(dis_F<30) { |
MR_Kang | 0:d429f13fe4be | 154 | move =1; measure =1; |
MR_Kang | 0:d429f13fe4be | 155 | } |
MR_Kang | 0:d429f13fe4be | 156 | break; |
MR_Kang | 0:d429f13fe4be | 157 | } |
MR_Kang | 0:d429f13fe4be | 158 | |
MR_Kang | 0:d429f13fe4be | 159 | |
MR_Kang | 0:d429f13fe4be | 160 | if(dis_R <10){move=5;} |
MR_Kang | 0:d429f13fe4be | 161 | else if(dis_L <10){ move =6;} |
MR_Kang | 0:d429f13fe4be | 162 | else if((dis_R <25) &&(dis_L<25)){move =4;} |
MR_Kang | 0:d429f13fe4be | 163 | else if(dis_R <35){move =2;} |
MR_Kang | 0:d429f13fe4be | 164 | else if(dis_L <35){move =3;} |
MR_Kang | 0:d429f13fe4be | 165 | |
MR_Kang | 0:d429f13fe4be | 166 | switch(move) { |
MR_Kang | 0:d429f13fe4be | 167 | case 0://Front |
MR_Kang | 0:d429f13fe4be | 168 | Front(); |
MR_Kang | 0:d429f13fe4be | 169 | measure =0; |
MR_Kang | 0:d429f13fe4be | 170 | break; |
MR_Kang | 0:d429f13fe4be | 171 | |
MR_Kang | 0:d429f13fe4be | 172 | case 1://Back |
MR_Kang | 0:d429f13fe4be | 173 | Stop(); |
MR_Kang | 0:d429f13fe4be | 174 | measure =0; |
MR_Kang | 0:d429f13fe4be | 175 | break; |
MR_Kang | 0:d429f13fe4be | 176 | |
MR_Kang | 0:d429f13fe4be | 177 | case 2://Right |
MR_Kang | 0:d429f13fe4be | 178 | Turn_R(); |
MR_Kang | 0:d429f13fe4be | 179 | if (flag==1) { |
MR_Kang | 0:d429f13fe4be | 180 | flag=0; |
MR_Kang | 0:d429f13fe4be | 181 | motortime.attach(&count,1); |
MR_Kang | 0:d429f13fe4be | 182 | } |
MR_Kang | 0:d429f13fe4be | 183 | wait(1); |
MR_Kang | 0:d429f13fe4be | 184 | break; |
MR_Kang | 0:d429f13fe4be | 185 | |
MR_Kang | 0:d429f13fe4be | 186 | case 3://Left |
MR_Kang | 0:d429f13fe4be | 187 | Turn_L(); |
MR_Kang | 0:d429f13fe4be | 188 | if (flag==1) { |
MR_Kang | 0:d429f13fe4be | 189 | flag=0; |
MR_Kang | 0:d429f13fe4be | 190 | motortime.attach(&count,1); |
MR_Kang | 0:d429f13fe4be | 191 | } |
MR_Kang | 0:d429f13fe4be | 192 | wait(1); |
MR_Kang | 0:d429f13fe4be | 193 | break; |
MR_Kang | 0:d429f13fe4be | 194 | |
MR_Kang | 0:d429f13fe4be | 195 | case 4://back |
MR_Kang | 0:d429f13fe4be | 196 | Back(); |
MR_Kang | 0:d429f13fe4be | 197 | backmotortime.attach(&backcount,2); |
MR_Kang | 0:d429f13fe4be | 198 | break; |
MR_Kang | 0:d429f13fe4be | 199 | case 5://Right |
MR_Kang | 0:d429f13fe4be | 200 | Back(); |
MR_Kang | 0:d429f13fe4be | 201 | Rightmotortime.attach(&Rightcount,2); |
MR_Kang | 0:d429f13fe4be | 202 | break; |
MR_Kang | 0:d429f13fe4be | 203 | case 6://back |
MR_Kang | 0:d429f13fe4be | 204 | Back(); |
MR_Kang | 0:d429f13fe4be | 205 | Leftmotortime.attach(&Leftcount,2); |
MR_Kang | 0:d429f13fe4be | 206 | break; |
MR_Kang | 0:d429f13fe4be | 207 | } |
MR_Kang | 0:d429f13fe4be | 208 | } |
MR_Kang | 0:d429f13fe4be | 209 | |
MR_Kang | 0:d429f13fe4be | 210 | } |