kao yi / Mbed 2 deprecated BX-car

running car

Dependencies:   mbed mbed-rtos

Fork of Boboobooo by kao yi

main.cpp@2:c51647d3c14d, 2014-06-04 (annotated)

Committer:
backman
Date:
Wed Jun 04 12:56:52 2014 +0000
Revision:
2:c51647d3c14d
Parent:
1:82bc25a7b68b
Child:
3:c5f2281b3ed2
two cam

Who changed what in which revision?

UserRevisionLine numberNew 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 2:c51647d3c14d 10 BX_camera cam;
backman 0:68c173249c01 11
backman 0:68c173249c01 12 int main() {
backman 0:68c173249c01 13
backman 1:82bc25a7b68b 14 /*
backman 1:82bc25a7b68b 15 int black_va;
backman 1:82bc25a7b68b 16 int white_va;
backman 1:82bc25a7b68b 17 */
backman 1:82bc25a7b68b 18
backman 1:82bc25a7b68b 19
backman 1:82bc25a7b68b 20 pc.baud(115200);
backman 1:82bc25a7b68b 21
backman 1:82bc25a7b68b 22
backman 1:82bc25a7b68b 23
backman 1:82bc25a7b68b 24
backman 1:82bc25a7b68b 25 char find_type=0x00;
backman 1:82bc25a7b68b 26
backman 1:82bc25a7b68b 27 int b_end;
backman 1:82bc25a7b68b 28 int b_start;
backman 1:82bc25a7b68b 29
backman 1:82bc25a7b68b 30 int b2_end;
backman 1:82bc25a7b68b 31 int b2_start;
backman 1:82bc25a7b68b 32
backman 1:82bc25a7b68b 33
backman 1:82bc25a7b68b 34 int center;
backman 1:82bc25a7b68b 35 int j=64;
backman 1:82bc25a7b68b 36 bool f1;
backman 1:82bc25a7b68b 37 bool f2;
backman 1:82bc25a7b68b 38 bool f3;
backman 1:82bc25a7b68b 39 bool f4;
backman 1:82bc25a7b68b 40 while(1){
backman 1:82bc25a7b68b 41
backman 2:c51647d3c14d 42 //
backman 1:82bc25a7b68b 43 cam.read();
backman 1:82bc25a7b68b 44
backman 1:82bc25a7b68b 45 for(int i=0;i<128;i++){
backman 1:82bc25a7b68b 46 if(i==64)
backman 1:82bc25a7b68b 47 pc.printf("X");
backman 1:82bc25a7b68b 48 else
backman 2:c51647d3c14d 49 pc.printf("%c", cam.sign_line_imageL[i]);
backman 2:c51647d3c14d 50 }
backman 2:c51647d3c14d 51 pc.printf(" || ");
backman 2:c51647d3c14d 52
backman 2:c51647d3c14d 53 for(int i=0;i<128;i++){
backman 2:c51647d3c14d 54 if(i==64)
backman 2:c51647d3c14d 55 pc.printf("X");
backman 2:c51647d3c14d 56 else
backman 2:c51647d3c14d 57 pc.printf("%c", cam.sign_line_imageR[i]);
backman 1:82bc25a7b68b 58 }
backman 1:82bc25a7b68b 59 pc.printf("\r\n");
backman 1:82bc25a7b68b 60
backman 2:c51647d3c14d 61
backman 1:82bc25a7b68b 62
backman 1:82bc25a7b68b 63
backman 1:82bc25a7b68b 64
backman 1:82bc25a7b68b 65 // find center
backman 1:82bc25a7b68b 66 // case 1 // | //
backman 1:82bc25a7b68b 67 //case 2 / | /
backman 2:c51647d3c14d 68 /*
backman 1:82bc25a7b68b 69 if(cam.sign_line_image[64]==' ')
backman 1:82bc25a7b68b 70 find_type=0x02;
backman 1:82bc25a7b68b 71 else
backman 1:82bc25a7b68b 72 find_type=0x01;
backman 1:82bc25a7b68b 73
backman 1:82bc25a7b68b 74 b_start=0;
backman 1:82bc25a7b68b 75 b_end=0;
backman 1:82bc25a7b68b 76 b2_start=0;
backman 1:82bc25a7b68b 77 b2_end=0;
backman 1:82bc25a7b68b 78
backman 1:82bc25a7b68b 79 j=64;
backman 1:82bc25a7b68b 80 f1=false;
backman 1:82bc25a7b68b 81 f2=false;
backman 1:82bc25a7b68b 82 f3=false;
backman 1:82bc25a7b68b 83 f4=false;
backman 1:82bc25a7b68b 84 for(int i=64;i<128; i++,j--){
backman 1:82bc25a7b68b 85
backman 1:82bc25a7b68b 86
backman 1:82bc25a7b68b 87 switch(find_type){
backman 1:82bc25a7b68b 88
backman 1:82bc25a7b68b 89
backman 1:82bc25a7b68b 90 case 0x01:
backman 1:82bc25a7b68b 91
backman 1:82bc25a7b68b 92 if(f1==false&&cam.sign_line_image[i]==' '){
backman 1:82bc25a7b68b 93 if(f1==false){
backman 1:82bc25a7b68b 94 b_start=i;
backman 1:82bc25a7b68b 95 f1=true;
backman 1:82bc25a7b68b 96 }
backman 1:82bc25a7b68b 97
backman 1:82bc25a7b68b 98 }
backman 1:82bc25a7b68b 99 if(f1== true&& f2==false&&cam.sign_line_image[i]=='O'){
backman 1:82bc25a7b68b 100 if(f2==false){
backman 1:82bc25a7b68b 101 b_end=i-1;
backman 1:82bc25a7b68b 102 f2=true;
backman 1:82bc25a7b68b 103 }
backman 1:82bc25a7b68b 104
backman 1:82bc25a7b68b 105 }
backman 1:82bc25a7b68b 106
backman 1:82bc25a7b68b 107 if(f3==false&&cam.sign_line_image[j]==' '){
backman 1:82bc25a7b68b 108 if(f3==false){
backman 1:82bc25a7b68b 109 b2_end=j;
backman 1:82bc25a7b68b 110 f3=true;
backman 1:82bc25a7b68b 111 }
backman 1:82bc25a7b68b 112
backman 1:82bc25a7b68b 113 }
backman 1:82bc25a7b68b 114 if(f3==true&&f4==false&&cam.sign_line_image[j]=='O'){
backman 1:82bc25a7b68b 115 if(f4==false){
backman 1:82bc25a7b68b 116 b2_start=j-1;
backman 1:82bc25a7b68b 117 f4=true;
backman 1:82bc25a7b68b 118 }
backman 1:82bc25a7b68b 119
backman 1:82bc25a7b68b 120 }
backman 1:82bc25a7b68b 121
backman 1:82bc25a7b68b 122
backman 1:82bc25a7b68b 123
backman 1:82bc25a7b68b 124
backman 1:82bc25a7b68b 125
backman 1:82bc25a7b68b 126
backman 1:82bc25a7b68b 127
backman 1:82bc25a7b68b 128
backman 1:82bc25a7b68b 129 break;
backman 1:82bc25a7b68b 130
backman 1:82bc25a7b68b 131
backman 1:82bc25a7b68b 132 case 0x02:
backman 1:82bc25a7b68b 133
backman 1:82bc25a7b68b 134 if(cam.sign_line_image[i]=='O'){
backman 1:82bc25a7b68b 135
backman 1:82bc25a7b68b 136 if(f1==false){
backman 1:82bc25a7b68b 137 b_end=i;
backman 1:82bc25a7b68b 138 f1=true;
backman 1:82bc25a7b68b 139 }
backman 1:82bc25a7b68b 140 }
backman 1:82bc25a7b68b 141
backman 1:82bc25a7b68b 142 if(cam.sign_line_image[j]=='O'){
backman 1:82bc25a7b68b 143
backman 1:82bc25a7b68b 144 if(f2==false){
backman 1:82bc25a7b68b 145 b_start=j;
backman 1:82bc25a7b68b 146 f2=true;
backman 1:82bc25a7b68b 147 }
backman 1:82bc25a7b68b 148
backman 1:82bc25a7b68b 149 }
backman 1:82bc25a7b68b 150
backman 1:82bc25a7b68b 151
backman 1:82bc25a7b68b 152 break;
backman 1:82bc25a7b68b 153
backman 1:82bc25a7b68b 154
backman 1:82bc25a7b68b 155
backman 1:82bc25a7b68b 156
backman 1:82bc25a7b68b 157 }
backman 1:82bc25a7b68b 158
backman 1:82bc25a7b68b 159
backman 1:82bc25a7b68b 160 }
backman 1:82bc25a7b68b 161
backman 1:82bc25a7b68b 162
backman 1:82bc25a7b68b 163 switch(find_type){
backman 1:82bc25a7b68b 164
backman 1:82bc25a7b68b 165 case 0x01:
backman 1:82bc25a7b68b 166
backman 1:82bc25a7b68b 167 if((b_end-b_start)>(b2_end-b2_start) )
backman 1:82bc25a7b68b 168 center=(b_end+b_start)/2;
backman 1:82bc25a7b68b 169
backman 1:82bc25a7b68b 170 else
backman 1:82bc25a7b68b 171 center=(b2_end+b2_start)/2;
backman 1:82bc25a7b68b 172
backman 1:82bc25a7b68b 173 pc.printf("%d %d | %d %d\r\n",b_start,b_end,b2_start,b2_end);
backman 1:82bc25a7b68b 174
backman 1:82bc25a7b68b 175
backman 1:82bc25a7b68b 176 break;
backman 1:82bc25a7b68b 177
backman 1:82bc25a7b68b 178 case 0x02:
backman 1:82bc25a7b68b 179 center=(b_end+b_start)/2;
backman 1:82bc25a7b68b 180
backman 1:82bc25a7b68b 181 break;
backman 1:82bc25a7b68b 182 }
backman 2:c51647d3c14d 183 pc.printf("ang :%d\r\n ",( (64.0-center) /64.0 )*90);
backman 1:82bc25a7b68b 184 //--------------------------------------------
backman 2:c51647d3c14d 185
backman 2:c51647d3c14d 186
backman 2:c51647d3c14d 187 servo.set_angle(( (64.0-center) /64.0 )*90 );
backman 2:c51647d3c14d 188
backman 2:c51647d3c14d 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
backman 1:82bc25a7b68b 200
backman 1:82bc25a7b68b 201
backman 1:82bc25a7b68b 202
backman 1:82bc25a7b68b 203
backman 1:82bc25a7b68b 204
backman 1:82bc25a7b68b 205
backman 1:82bc25a7b68b 206
backman 1:82bc25a7b68b 207
backman 1:82bc25a7b68b 208
backman 1:82bc25a7b68b 209 //tmp
backman 1:82bc25a7b68b 210 int angle;
backman 1:82bc25a7b68b 211 float black_pR=0;
backman 1:82bc25a7b68b 212 float black_pL=0;
backman 1:82bc25a7b68b 213 float black_pT=0;
backman 1:82bc25a7b68b 214 //tune
backman 1:82bc25a7b68b 215 /*
backman 1:82bc25a7b68b 216 int min_va=90000;
backman 1:82bc25a7b68b 217 int max_va=0;
backman 1:82bc25a7b68b 218 int mid_va=0;
backman 1:82bc25a7b68b 219 */
backman 1:82bc25a7b68b 220
backman 1:82bc25a7b68b 221
backman 1:82bc25a7b68b 222
backman 1:82bc25a7b68b 223 while(1){
backman 1:82bc25a7b68b 224 black_pR=0;
backman 1:82bc25a7b68b 225 black_pL=0;
backman 1:82bc25a7b68b 226 black_pT=0;
backman 1:82bc25a7b68b 227 // min_va=90000;
backman 1:82bc25a7b68b 228 // max_va=0;
backman 1:82bc25a7b68b 229 angle=0;
backman 1:82bc25a7b68b 230 cam.read();
backman 1:82bc25a7b68b 231 for(int i=0;i<64;i++){
backman 1:82bc25a7b68b 232
backman 1:82bc25a7b68b 233
backman 1:82bc25a7b68b 234 // angle+=cam.line_image[i]-cam.line_image[127-i];
backman 2:c51647d3c14d 235 if(cam.line_imageR[i]==0)
backman 1:82bc25a7b68b 236 black_pR++;
backman 2:c51647d3c14d 237 if(cam.line_imageR[127-i]==0)
backman 1:82bc25a7b68b 238 black_pL++;
backman 1:82bc25a7b68b 239 }
backman 1:82bc25a7b68b 240
backman 1:82bc25a7b68b 241
backman 1:82bc25a7b68b 242 black_pT=black_pL+black_pR;
backman 1:82bc25a7b68b 243
backman 1:82bc25a7b68b 244
backman 1:82bc25a7b68b 245 if(black_pL>black_pT/2){
backman 1:82bc25a7b68b 246
backman 1:82bc25a7b68b 247 angle=(black_pR/black_pT)*90;
backman 1:82bc25a7b68b 248
backman 1:82bc25a7b68b 249 //angle=
backman 1:82bc25a7b68b 250 }
backman 1:82bc25a7b68b 251 else if(black_pR>black_pT/2){
backman 1:82bc25a7b68b 252
backman 1:82bc25a7b68b 253 angle=-1.0*(black_pR/black_pT)*90;
backman 1:82bc25a7b68b 254 //angle=-0.5;
backman 1:82bc25a7b68b 255 }
backman 1:82bc25a7b68b 256 else{
backman 1:82bc25a7b68b 257
backman 1:82bc25a7b68b 258 angle=0;
backman 1:82bc25a7b68b 259
backman 1:82bc25a7b68b 260 }
backman 1:82bc25a7b68b 261 servo.set_angle(angle);
backman 1:82bc25a7b68b 262 pc.printf("ang: %d",angle);
backman 1:82bc25a7b68b 263 pc.printf("\r\n");
backman 1:82bc25a7b68b 264
backman 1:82bc25a7b68b 265
backman 1:82bc25a7b68b 266
backman 1:82bc25a7b68b 267
backman 1:82bc25a7b68b 268
backman 1:82bc25a7b68b 269
backman 1:82bc25a7b68b 270
backman 1:82bc25a7b68b 271 }
backman 1:82bc25a7b68b 272
backman 1:82bc25a7b68b 273
backman 1:82bc25a7b68b 274
backman 1:82bc25a7b68b 275
backman 1:82bc25a7b68b 276
backman 1:82bc25a7b68b 277
backman 1:82bc25a7b68b 278
backman 1:82bc25a7b68b 279 // pc.printf("mid: %d|min: %d | max: %d | diff: %d",mid_va,min_va,max_va,max_va-min_va);
backman 1:82bc25a7b68b 280
backman 1:82bc25a7b68b 281
backman 1:82bc25a7b68b 282
backman 1:82bc25a7b68b 283
backman 1:82bc25a7b68b 284
backman 1:82bc25a7b68b 285 //p - controller
backman 1:82bc25a7b68b 286
backman 1:82bc25a7b68b 287 //servo angle not yet contorl
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 1:82bc25a7b68b 305
backman 1:82bc25a7b68b 306
backman 1:82bc25a7b68b 307
backman 1:82bc25a7b68b 308
backman 1:82bc25a7b68b 309
backman 1:82bc25a7b68b 310
backman 1:82bc25a7b68b 311
backman 1:82bc25a7b68b 312
backman 1:82bc25a7b68b 313
backman 1:82bc25a7b68b 314
backman 0:68c173249c01 315
backman 0:68c173249c01 316
backman 0:68c173249c01 317
backman 1:82bc25a7b68b 318 return 0;
backman 0:68c173249c01 319
backman 0:68c173249c01 320
backman 0:68c173249c01 321 }