kao yi / Mbed 2 deprecated BX-car

running car

Dependencies:   mbed mbed-rtos

Fork of Boboobooo by kao yi

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?

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 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 }