譯文 張
/
7_7Boboobooo
123
Fork of Boboobooo by
main.cpp@1:82bc25a7b68b, 2014-06-03 (annotated)
- Committer:
- backman
- Date:
- Tue Jun 03 15:53:55 2014 +0000
- Revision:
- 1:82bc25a7b68b
- Parent:
- 0:68c173249c01
- Child:
- 2:c51647d3c14d
cam_algroithm
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
backman | 0:68c173249c01 | 1 | #include "mbed.h" |
backman | 1:82bc25a7b68b | 2 | #include "servo_api.h" |
backman | 1:82bc25a7b68b | 3 | #include "camera_api.h" |
backman | 1:82bc25a7b68b | 4 | //BX_servo servo; |
backman | 0:68c173249c01 | 5 | |
backman | 0:68c173249c01 | 6 | |
backman | 1:82bc25a7b68b | 7 | Serial pc(USBTX, USBRX); |
backman | 1:82bc25a7b68b | 8 | BX_servo servo; |
backman | 1:82bc25a7b68b | 9 | |
backman | 1:82bc25a7b68b | 10 | BX_camera cam('n'); |
backman | 1:82bc25a7b68b | 11 | PwmOut cam_clk(PTC3); |
backman | 0:68c173249c01 | 12 | |
backman | 0:68c173249c01 | 13 | int main() { |
backman | 0:68c173249c01 | 14 | |
backman | 1:82bc25a7b68b | 15 | /* |
backman | 1:82bc25a7b68b | 16 | int black_va; |
backman | 1:82bc25a7b68b | 17 | int white_va; |
backman | 1:82bc25a7b68b | 18 | */ |
backman | 1:82bc25a7b68b | 19 | |
backman | 1:82bc25a7b68b | 20 | |
backman | 1:82bc25a7b68b | 21 | pc.baud(115200); |
backman | 1:82bc25a7b68b | 22 | |
backman | 1:82bc25a7b68b | 23 | |
backman | 1:82bc25a7b68b | 24 | |
backman | 1:82bc25a7b68b | 25 | |
backman | 1:82bc25a7b68b | 26 | char find_type=0x00; |
backman | 1:82bc25a7b68b | 27 | |
backman | 1:82bc25a7b68b | 28 | int b_end; |
backman | 1:82bc25a7b68b | 29 | int b_start; |
backman | 1:82bc25a7b68b | 30 | |
backman | 1:82bc25a7b68b | 31 | int b2_end; |
backman | 1:82bc25a7b68b | 32 | int b2_start; |
backman | 1:82bc25a7b68b | 33 | |
backman | 1:82bc25a7b68b | 34 | |
backman | 1:82bc25a7b68b | 35 | int center; |
backman | 1:82bc25a7b68b | 36 | int j=64; |
backman | 1:82bc25a7b68b | 37 | bool f1; |
backman | 1:82bc25a7b68b | 38 | bool f2; |
backman | 1:82bc25a7b68b | 39 | bool f3; |
backman | 1:82bc25a7b68b | 40 | bool f4; |
backman | 1:82bc25a7b68b | 41 | while(1){ |
backman | 1:82bc25a7b68b | 42 | |
backman | 1:82bc25a7b68b | 43 | |
backman | 1:82bc25a7b68b | 44 | cam.read(); |
backman | 1:82bc25a7b68b | 45 | |
backman | 1:82bc25a7b68b | 46 | for(int i=0;i<128;i++){ |
backman | 1:82bc25a7b68b | 47 | if(i==64) |
backman | 1:82bc25a7b68b | 48 | pc.printf("X"); |
backman | 1:82bc25a7b68b | 49 | else |
backman | 1:82bc25a7b68b | 50 | pc.printf("%c", cam.sign_line_image[i]); |
backman | 1:82bc25a7b68b | 51 | } |
backman | 1:82bc25a7b68b | 52 | pc.printf("\r\n"); |
backman | 1:82bc25a7b68b | 53 | |
backman | 1:82bc25a7b68b | 54 | |
backman | 1:82bc25a7b68b | 55 | |
backman | 1:82bc25a7b68b | 56 | |
backman | 1:82bc25a7b68b | 57 | // find center |
backman | 1:82bc25a7b68b | 58 | // case 1 // | // |
backman | 1:82bc25a7b68b | 59 | //case 2 / | / |
backman | 1:82bc25a7b68b | 60 | |
backman | 1:82bc25a7b68b | 61 | if(cam.sign_line_image[64]==' ') |
backman | 1:82bc25a7b68b | 62 | find_type=0x02; |
backman | 1:82bc25a7b68b | 63 | else |
backman | 1:82bc25a7b68b | 64 | find_type=0x01; |
backman | 1:82bc25a7b68b | 65 | |
backman | 1:82bc25a7b68b | 66 | b_start=0; |
backman | 1:82bc25a7b68b | 67 | b_end=0; |
backman | 1:82bc25a7b68b | 68 | b2_start=0; |
backman | 1:82bc25a7b68b | 69 | b2_end=0; |
backman | 1:82bc25a7b68b | 70 | |
backman | 1:82bc25a7b68b | 71 | j=64; |
backman | 1:82bc25a7b68b | 72 | f1=false; |
backman | 1:82bc25a7b68b | 73 | f2=false; |
backman | 1:82bc25a7b68b | 74 | f3=false; |
backman | 1:82bc25a7b68b | 75 | f4=false; |
backman | 1:82bc25a7b68b | 76 | for(int i=64;i<128; i++,j--){ |
backman | 1:82bc25a7b68b | 77 | |
backman | 1:82bc25a7b68b | 78 | |
backman | 1:82bc25a7b68b | 79 | switch(find_type){ |
backman | 1:82bc25a7b68b | 80 | |
backman | 1:82bc25a7b68b | 81 | |
backman | 1:82bc25a7b68b | 82 | case 0x01: |
backman | 1:82bc25a7b68b | 83 | |
backman | 1:82bc25a7b68b | 84 | if(f1==false&&cam.sign_line_image[i]==' '){ |
backman | 1:82bc25a7b68b | 85 | if(f1==false){ |
backman | 1:82bc25a7b68b | 86 | b_start=i; |
backman | 1:82bc25a7b68b | 87 | f1=true; |
backman | 1:82bc25a7b68b | 88 | } |
backman | 1:82bc25a7b68b | 89 | |
backman | 1:82bc25a7b68b | 90 | } |
backman | 1:82bc25a7b68b | 91 | if(f1== true&& f2==false&&cam.sign_line_image[i]=='O'){ |
backman | 1:82bc25a7b68b | 92 | if(f2==false){ |
backman | 1:82bc25a7b68b | 93 | b_end=i-1; |
backman | 1:82bc25a7b68b | 94 | f2=true; |
backman | 1:82bc25a7b68b | 95 | } |
backman | 1:82bc25a7b68b | 96 | |
backman | 1:82bc25a7b68b | 97 | } |
backman | 1:82bc25a7b68b | 98 | |
backman | 1:82bc25a7b68b | 99 | if(f3==false&&cam.sign_line_image[j]==' '){ |
backman | 1:82bc25a7b68b | 100 | if(f3==false){ |
backman | 1:82bc25a7b68b | 101 | b2_end=j; |
backman | 1:82bc25a7b68b | 102 | f3=true; |
backman | 1:82bc25a7b68b | 103 | } |
backman | 1:82bc25a7b68b | 104 | |
backman | 1:82bc25a7b68b | 105 | } |
backman | 1:82bc25a7b68b | 106 | if(f3==true&&f4==false&&cam.sign_line_image[j]=='O'){ |
backman | 1:82bc25a7b68b | 107 | if(f4==false){ |
backman | 1:82bc25a7b68b | 108 | b2_start=j-1; |
backman | 1:82bc25a7b68b | 109 | f4=true; |
backman | 1:82bc25a7b68b | 110 | } |
backman | 1:82bc25a7b68b | 111 | |
backman | 1:82bc25a7b68b | 112 | } |
backman | 1:82bc25a7b68b | 113 | |
backman | 1:82bc25a7b68b | 114 | |
backman | 1:82bc25a7b68b | 115 | |
backman | 1:82bc25a7b68b | 116 | |
backman | 1:82bc25a7b68b | 117 | |
backman | 1:82bc25a7b68b | 118 | |
backman | 1:82bc25a7b68b | 119 | |
backman | 1:82bc25a7b68b | 120 | |
backman | 1:82bc25a7b68b | 121 | break; |
backman | 1:82bc25a7b68b | 122 | |
backman | 1:82bc25a7b68b | 123 | |
backman | 1:82bc25a7b68b | 124 | case 0x02: |
backman | 1:82bc25a7b68b | 125 | |
backman | 1:82bc25a7b68b | 126 | if(cam.sign_line_image[i]=='O'){ |
backman | 1:82bc25a7b68b | 127 | |
backman | 1:82bc25a7b68b | 128 | if(f1==false){ |
backman | 1:82bc25a7b68b | 129 | b_end=i; |
backman | 1:82bc25a7b68b | 130 | f1=true; |
backman | 1:82bc25a7b68b | 131 | } |
backman | 1:82bc25a7b68b | 132 | } |
backman | 1:82bc25a7b68b | 133 | |
backman | 1:82bc25a7b68b | 134 | if(cam.sign_line_image[j]=='O'){ |
backman | 1:82bc25a7b68b | 135 | |
backman | 1:82bc25a7b68b | 136 | if(f2==false){ |
backman | 1:82bc25a7b68b | 137 | b_start=j; |
backman | 1:82bc25a7b68b | 138 | f2=true; |
backman | 1:82bc25a7b68b | 139 | } |
backman | 1:82bc25a7b68b | 140 | |
backman | 1:82bc25a7b68b | 141 | } |
backman | 1:82bc25a7b68b | 142 | |
backman | 1:82bc25a7b68b | 143 | |
backman | 1:82bc25a7b68b | 144 | break; |
backman | 1:82bc25a7b68b | 145 | |
backman | 1:82bc25a7b68b | 146 | |
backman | 1:82bc25a7b68b | 147 | |
backman | 1:82bc25a7b68b | 148 | |
backman | 1:82bc25a7b68b | 149 | } |
backman | 1:82bc25a7b68b | 150 | |
backman | 1:82bc25a7b68b | 151 | |
backman | 1:82bc25a7b68b | 152 | } |
backman | 1:82bc25a7b68b | 153 | |
backman | 1:82bc25a7b68b | 154 | |
backman | 1:82bc25a7b68b | 155 | switch(find_type){ |
backman | 1:82bc25a7b68b | 156 | |
backman | 1:82bc25a7b68b | 157 | case 0x01: |
backman | 1:82bc25a7b68b | 158 | |
backman | 1:82bc25a7b68b | 159 | if((b_end-b_start)>(b2_end-b2_start) ) |
backman | 1:82bc25a7b68b | 160 | center=(b_end+b_start)/2; |
backman | 1:82bc25a7b68b | 161 | |
backman | 1:82bc25a7b68b | 162 | else |
backman | 1:82bc25a7b68b | 163 | center=(b2_end+b2_start)/2; |
backman | 1:82bc25a7b68b | 164 | |
backman | 1:82bc25a7b68b | 165 | pc.printf("%d %d | %d %d\r\n",b_start,b_end,b2_start,b2_end); |
backman | 1:82bc25a7b68b | 166 | |
backman | 1:82bc25a7b68b | 167 | |
backman | 1:82bc25a7b68b | 168 | break; |
backman | 1:82bc25a7b68b | 169 | |
backman | 1:82bc25a7b68b | 170 | case 0x02: |
backman | 1:82bc25a7b68b | 171 | center=(b_end+b_start)/2; |
backman | 1:82bc25a7b68b | 172 | |
backman | 1:82bc25a7b68b | 173 | break; |
backman | 1:82bc25a7b68b | 174 | } |
backman | 1:82bc25a7b68b | 175 | pc.printf("center :%d\r\n",center); |
backman | 1:82bc25a7b68b | 176 | //-------------------------------------------- |
backman | 1:82bc25a7b68b | 177 | |
backman | 1:82bc25a7b68b | 178 | |
backman | 1:82bc25a7b68b | 179 | |
backman | 1:82bc25a7b68b | 180 | |
backman | 1:82bc25a7b68b | 181 | } |
backman | 1:82bc25a7b68b | 182 | |
backman | 1:82bc25a7b68b | 183 | |
backman | 1:82bc25a7b68b | 184 | |
backman | 1:82bc25a7b68b | 185 | |
backman | 1:82bc25a7b68b | 186 | |
backman | 1:82bc25a7b68b | 187 | |
backman | 1:82bc25a7b68b | 188 | |
backman | 1:82bc25a7b68b | 189 | |
backman | 1:82bc25a7b68b | 190 | |
backman | 1:82bc25a7b68b | 191 | |
backman | 1:82bc25a7b68b | 192 | |
backman | 1:82bc25a7b68b | 193 | |
backman | 1:82bc25a7b68b | 194 | |
backman | 1:82bc25a7b68b | 195 | |
backman | 1:82bc25a7b68b | 196 | |
backman | 1:82bc25a7b68b | 197 | |
backman | 1:82bc25a7b68b | 198 | |
backman | 1:82bc25a7b68b | 199 | //tmp |
backman | 1:82bc25a7b68b | 200 | int angle; |
backman | 1:82bc25a7b68b | 201 | float black_pR=0; |
backman | 1:82bc25a7b68b | 202 | float black_pL=0; |
backman | 1:82bc25a7b68b | 203 | float black_pT=0; |
backman | 1:82bc25a7b68b | 204 | //tune |
backman | 1:82bc25a7b68b | 205 | /* |
backman | 1:82bc25a7b68b | 206 | int min_va=90000; |
backman | 1:82bc25a7b68b | 207 | int max_va=0; |
backman | 1:82bc25a7b68b | 208 | int mid_va=0; |
backman | 1:82bc25a7b68b | 209 | */ |
backman | 1:82bc25a7b68b | 210 | |
backman | 1:82bc25a7b68b | 211 | |
backman | 1:82bc25a7b68b | 212 | |
backman | 1:82bc25a7b68b | 213 | while(1){ |
backman | 1:82bc25a7b68b | 214 | black_pR=0; |
backman | 1:82bc25a7b68b | 215 | black_pL=0; |
backman | 1:82bc25a7b68b | 216 | black_pT=0; |
backman | 1:82bc25a7b68b | 217 | // min_va=90000; |
backman | 1:82bc25a7b68b | 218 | // max_va=0; |
backman | 1:82bc25a7b68b | 219 | angle=0; |
backman | 1:82bc25a7b68b | 220 | cam.read(); |
backman | 1:82bc25a7b68b | 221 | for(int i=0;i<64;i++){ |
backman | 1:82bc25a7b68b | 222 | |
backman | 1:82bc25a7b68b | 223 | |
backman | 1:82bc25a7b68b | 224 | // angle+=cam.line_image[i]-cam.line_image[127-i]; |
backman | 1:82bc25a7b68b | 225 | if(cam.line_image[i]==0) |
backman | 1:82bc25a7b68b | 226 | black_pR++; |
backman | 1:82bc25a7b68b | 227 | if(cam.line_image[127-i]==0) |
backman | 1:82bc25a7b68b | 228 | black_pL++; |
backman | 1:82bc25a7b68b | 229 | } |
backman | 1:82bc25a7b68b | 230 | |
backman | 1:82bc25a7b68b | 231 | |
backman | 1:82bc25a7b68b | 232 | black_pT=black_pL+black_pR; |
backman | 1:82bc25a7b68b | 233 | |
backman | 1:82bc25a7b68b | 234 | |
backman | 1:82bc25a7b68b | 235 | if(black_pL>black_pT/2){ |
backman | 1:82bc25a7b68b | 236 | |
backman | 1:82bc25a7b68b | 237 | angle=(black_pR/black_pT)*90; |
backman | 1:82bc25a7b68b | 238 | |
backman | 1:82bc25a7b68b | 239 | //angle= |
backman | 1:82bc25a7b68b | 240 | } |
backman | 1:82bc25a7b68b | 241 | else if(black_pR>black_pT/2){ |
backman | 1:82bc25a7b68b | 242 | |
backman | 1:82bc25a7b68b | 243 | angle=-1.0*(black_pR/black_pT)*90; |
backman | 1:82bc25a7b68b | 244 | //angle=-0.5; |
backman | 1:82bc25a7b68b | 245 | } |
backman | 1:82bc25a7b68b | 246 | else{ |
backman | 1:82bc25a7b68b | 247 | |
backman | 1:82bc25a7b68b | 248 | angle=0; |
backman | 1:82bc25a7b68b | 249 | |
backman | 1:82bc25a7b68b | 250 | } |
backman | 1:82bc25a7b68b | 251 | servo.set_angle(angle); |
backman | 1:82bc25a7b68b | 252 | pc.printf("ang: %d",angle); |
backman | 1:82bc25a7b68b | 253 | pc.printf("\r\n"); |
backman | 1:82bc25a7b68b | 254 | |
backman | 1:82bc25a7b68b | 255 | |
backman | 1:82bc25a7b68b | 256 | |
backman | 1:82bc25a7b68b | 257 | |
backman | 1:82bc25a7b68b | 258 | |
backman | 1:82bc25a7b68b | 259 | |
backman | 1:82bc25a7b68b | 260 | |
backman | 1:82bc25a7b68b | 261 | } |
backman | 1:82bc25a7b68b | 262 | |
backman | 1:82bc25a7b68b | 263 | |
backman | 1:82bc25a7b68b | 264 | |
backman | 1:82bc25a7b68b | 265 | |
backman | 1:82bc25a7b68b | 266 | |
backman | 1:82bc25a7b68b | 267 | |
backman | 1:82bc25a7b68b | 268 | |
backman | 1:82bc25a7b68b | 269 | // pc.printf("mid: %d|min: %d | max: %d | diff: %d",mid_va,min_va,max_va,max_va-min_va); |
backman | 1:82bc25a7b68b | 270 | |
backman | 1:82bc25a7b68b | 271 | |
backman | 1:82bc25a7b68b | 272 | |
backman | 1:82bc25a7b68b | 273 | |
backman | 1:82bc25a7b68b | 274 | |
backman | 1:82bc25a7b68b | 275 | //p - controller |
backman | 1:82bc25a7b68b | 276 | |
backman | 1:82bc25a7b68b | 277 | //servo angle not yet contorl |
backman | 1:82bc25a7b68b | 278 | |
backman | 1:82bc25a7b68b | 279 | |
backman | 1:82bc25a7b68b | 280 | |
backman | 1:82bc25a7b68b | 281 | |
backman | 1:82bc25a7b68b | 282 | |
backman | 1:82bc25a7b68b | 283 | |
backman | 1:82bc25a7b68b | 284 | |
backman | 1:82bc25a7b68b | 285 | |
backman | 1:82bc25a7b68b | 286 | |
backman | 1:82bc25a7b68b | 287 | |
backman | 1:82bc25a7b68b | 288 | |
backman | 1:82bc25a7b68b | 289 | |
backman | 1:82bc25a7b68b | 290 | |
backman | 1:82bc25a7b68b | 291 | |
backman | 1:82bc25a7b68b | 292 | |
backman | 1:82bc25a7b68b | 293 | |
backman | 1:82bc25a7b68b | 294 | |
backman | 1:82bc25a7b68b | 295 | |
backman | 1:82bc25a7b68b | 296 | |
backman | 1:82bc25a7b68b | 297 | |
backman | 1:82bc25a7b68b | 298 | |
backman | 1:82bc25a7b68b | 299 | |
backman | 1:82bc25a7b68b | 300 | |
backman | 1:82bc25a7b68b | 301 | |
backman | 1:82bc25a7b68b | 302 | |
backman | 1:82bc25a7b68b | 303 | |
backman | 1:82bc25a7b68b | 304 | |
backman | 0:68c173249c01 | 305 | |
backman | 0:68c173249c01 | 306 | |
backman | 0:68c173249c01 | 307 | |
backman | 1:82bc25a7b68b | 308 | return 0; |
backman | 0:68c173249c01 | 309 | |
backman | 0:68c173249c01 | 310 | |
backman | 0:68c173249c01 | 311 | } |