chia-hsun wu / Mbed 2 deprecated Boboobooov10

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-rtos mbed

Fork of Boboobooov4 by kao yi

main.cpp@21:4e8a4f66aaef, 2014-07-10 (annotated)

Committer:
fhcrcmars
Date:
Thu Jul 10 03:25:59 2014 +0000
Revision:
21:4e8a4f66aaef
Parent:
20:f541b6b063fa
Child:
22:34a0c436ac45
new

Who changed what in which revision?

UserRevisionLine numberNew contents of line
backman 0:68c173249c01 1 #include "mbed.h"
backman 11:418e39749f48 2 #include "rtos.h"
backman 7:fd976e1ced33 3 #include "controller.h"
backman 1:82bc25a7b68b 4 #include "servo_api.h"
backman 1:82bc25a7b68b 5 #include "camera_api.h"
backman 6:5a39bde2e016 6 #include "motor_api.h"
backman 7:fd976e1ced33 7 #include "pot.h"
backman 11:418e39749f48 8 #include "TSISensor.h"
backman 18:eb675df59c7f 9 #include "Stack.h"
backman 3:c5f2281b3ed2 10
backman 3:c5f2281b3ed2 11 #define Debug_cam_uart
fhcrcmars 20:f541b6b063fa 12 #define L_eye
backman 19:4869b10a962e 13 #define R_eye
backman 7:fd976e1ced33 14 #define motor_on
backman 6:5a39bde2e016 15 #define Pcontroller
backman 19:4869b10a962e 16 //#define task_ma_time
backman 11:418e39749f48 17
backman 14:2d90b0066fc6 18
fhcrcmars 20:f541b6b063fa 19 #define center 64
backman 14:2d90b0066fc6 20
backman 14:2d90b0066fc6 21
backman 14:2d90b0066fc6 22
fhcrcmars 20:f541b6b063fa 23 #define t_cam 4
backman 11:418e39749f48 24
backman 19:4869b10a962e 25 #define black_center 64
backman 7:fd976e1ced33 26
backman 11:418e39749f48 27
backman 12:fdada4af384a 28 Serial pc(USBTX, USBRX); // tx, rx
backman 11:418e39749f48 29
backman 0:68c173249c01 30
backman 0:68c173249c01 31
backman 1:82bc25a7b68b 32 BX_servo servo;
backman 6:5a39bde2e016 33
backman 14:2d90b0066fc6 34 BX_camera cam(0);
backman 0:68c173249c01 35
backman 6:5a39bde2e016 36 BX_motor MotorA('A');
backman 6:5a39bde2e016 37 BX_motor MotorB('B');
backman 3:c5f2281b3ed2 38
backman 7:fd976e1ced33 39 BX_pot pot1('1');
backman 15:585df3979be8 40 BX_pot pot2('2');
backman 7:fd976e1ced33 41
backman 9:33b99cb45e99 42 // 90/30=3
backman 15:585df3979be8 43 PID cam_to_M_ctrlr(10.0,118.0,0.046,0.083,0.083-0.046,0.00,0.00,10);
backman 7:fd976e1ced33 44
backman 18:eb675df59c7f 45 #ifdef task_ma_time
backman 19:4869b10a962e 46 DigitalOut cam_p(PTE5); //cam black
backman 19:4869b10a962e 47 DigitalOut servo_p(PTE4); //servo coffee
backman 19:4869b10a962e 48 DigitalOut idle_p (PTE3);
backman 19:4869b10a962e 49 //DigitalOut de_p(PTD3); // red
backman 18:eb675df59c7f 50 #endif
backman 9:33b99cb45e99 51 TSISensor tsi;
backman 11:418e39749f48 52
backman 11:418e39749f48 53 //os
backman 11:418e39749f48 54 Mutex stdio_mutex;
backman 11:418e39749f48 55
backman 11:418e39749f48 56
backman 14:2d90b0066fc6 57
backman 18:eb675df59c7f 58 //global resource
backman 18:eb675df59c7f 59
fhcrcmars 20:f541b6b063fa 60 Stack<int> pointsR(10);
fhcrcmars 20:f541b6b063fa 61 Stack<int> pointsL(10);
backman 18:eb675df59c7f 62
backman 18:eb675df59c7f 63
backman 18:eb675df59c7f 64
backman 14:2d90b0066fc6 65
backman 14:2d90b0066fc6 66
backman 11:418e39749f48 67
backman 19:4869b10a962e 68
backman 11:418e39749f48 69 static double v_motor;
backman 11:418e39749f48 70 static double v_servo;
backman 11:418e39749f48 71
backman 15:585df3979be8 72
backman 15:585df3979be8 73
backman 15:585df3979be8 74
backman 15:585df3979be8 75
backman 11:418e39749f48 76 void cam_thread(void const *args){
backman 9:33b99cb45e99 77
backman 11:418e39749f48 78 while(true){
backman 18:eb675df59c7f 79 #ifdef task_ma_time
backman 18:eb675df59c7f 80 cam_p=1;
backman 19:4869b10a962e 81 #endif
backman 19:4869b10a962e 82
backman 15:585df3979be8 83 cam.read();
backman 11:418e39749f48 84
backman 19:4869b10a962e 85
backman 18:eb675df59c7f 86 #ifdef task_ma_time
backman 18:eb675df59c7f 87 cam_p=0;
backman 18:eb675df59c7f 88 #endif
backman 19:4869b10a962e 89
backman 19:4869b10a962e 90 int b_r_c,b_l_c;
backman 19:4869b10a962e 91
backman 19:4869b10a962e 92
backman 19:4869b10a962e 93 b_r_c=cam.black_centerR();
fhcrcmars 20:f541b6b063fa 94 b_l_c=cam.black_centerL();
backman 19:4869b10a962e 95
backman 19:4869b10a962e 96 //right
fhcrcmars 21:4e8a4f66aaef 97 //printf("push : R: %d L: %d \r\n",b_r_c,b_l_c);
fhcrcmars 20:f541b6b063fa 98 pointsR.push(b_r_c);
fhcrcmars 21:4e8a4f66aaef 99 pointsL.push(b_l_c);
backman 19:4869b10a962e 100
backman 18:eb675df59c7f 101
backman 18:eb675df59c7f 102
backman 15:585df3979be8 103 Thread::wait(t_cam);
backman 19:4869b10a962e 104
backman 11:418e39749f48 105 }
backman 11:418e39749f48 106
backman 11:418e39749f48 107 }
backman 11:418e39749f48 108 // function
backman 19:4869b10a962e 109
backman 19:4869b10a962e 110
backman 19:4869b10a962e 111
backman 19:4869b10a962e 112
backman 19:4869b10a962e 113
backman 19:4869b10a962e 114
backman 19:4869b10a962e 115
backman 19:4869b10a962e 116
backman 11:418e39749f48 117 void de_thread(void const *args){
backman 9:33b99cb45e99 118
backman 11:418e39749f48 119 while(1){
backman 11:418e39749f48 120
backman 18:eb675df59c7f 121
backman 18:eb675df59c7f 122 #ifdef task_ma_time
backman 19:4869b10a962e 123 //de_p=0;
backman 18:eb675df59c7f 124 #endif
backman 11:418e39749f48 125
backman 19:4869b10a962e 126 cam.read();
backman 11:418e39749f48 127 stdio_mutex.lock();
backman 11:418e39749f48 128 #ifdef Debug_cam_uart
backman 11:418e39749f48 129 #ifdef L_eye
backman 17:3dac99cf2b89 130 pc.printf("L: ");
fhcrcmars 20:f541b6b063fa 131 for(int i=127;i>=0;i--){
fhcrcmars 20:f541b6b063fa 132 if(i==64||i==0||i==127)
backman 12:fdada4af384a 133 pc.printf("X");
backman 11:418e39749f48 134 else
backman 13:63f9a5101205 135 pc.printf("%c", cam.sign_line_imageL[i]);
backman 11:418e39749f48 136 }
backman 13:63f9a5101205 137
fhcrcmars 20:f541b6b063fa 138 pc.printf(" || ");
backman 11:418e39749f48 139 #endif
backman 11:418e39749f48 140 #ifdef R_eye
backman 12:fdada4af384a 141 pc.printf("R: ");
fhcrcmars 20:f541b6b063fa 142 for(int i=127;i>=0;i--){
fhcrcmars 20:f541b6b063fa 143 if(i==64||i==0||i==127)
backman 12:fdada4af384a 144 pc.printf("X");
backman 11:418e39749f48 145 else
backman 12:fdada4af384a 146 pc.printf("%c", cam.sign_line_imageR[i]);
backman 11:418e39749f48 147 }
backman 17:3dac99cf2b89 148 #endif
backman 17:3dac99cf2b89 149
backman 17:3dac99cf2b89 150 pc.printf("\r\n Rcenter :%d Lcenter : %d servo: %f \r\n",cam.black_centerR(),cam.black_centerL(),v_servo);
backman 17:3dac99cf2b89 151
backman 15:585df3979be8 152
fhcrcmars 20:f541b6b063fa 153 // pc.printf("stack n: %d",points.available());
backman 19:4869b10a962e 154
backman 11:418e39749f48 155 stdio_mutex.unlock();
backman 1:82bc25a7b68b 156
backman 1:82bc25a7b68b 157
backman 18:eb675df59c7f 158
backman 18:eb675df59c7f 159 #ifdef task_ma_time
backman 19:4869b10a962e 160 //de_p=1;
backman 18:eb675df59c7f 161 #endif
backman 11:418e39749f48 162
backman 7:fd976e1ced33 163
backman 7:fd976e1ced33 164
backman 11:418e39749f48 165 #endif
backman 19:4869b10a962e 166 Thread::wait(10);
backman 7:fd976e1ced33 167
backman 11:418e39749f48 168
backman 11:418e39749f48 169 }
backman 11:418e39749f48 170
backman 11:418e39749f48 171 }
backman 11:418e39749f48 172
backman 11:418e39749f48 173
backman 7:fd976e1ced33 174
backman 7:fd976e1ced33 175
backman 11:418e39749f48 176 void servo_thread(void const *args){
backman 11:418e39749f48 177
backman 11:418e39749f48 178
backman 11:418e39749f48 179 while(1){
backman 18:eb675df59c7f 180
backman 18:eb675df59c7f 181 #ifdef task_ma_time
backman 18:eb675df59c7f 182 servo_p=1;
backman 18:eb675df59c7f 183 #endif
backman 15:585df3979be8 184
fhcrcmars 21:4e8a4f66aaef 185 // servo.set_angle(0.055);
backman 19:4869b10a962e 186 int point;
fhcrcmars 20:f541b6b063fa 187 int sum_error_R=0;
fhcrcmars 20:f541b6b063fa 188 int sum_error_L=0;
fhcrcmars 20:f541b6b063fa 189 int n_pointR=0;
fhcrcmars 20:f541b6b063fa 190 int n_pointL=0;
fhcrcmars 20:f541b6b063fa 191 n_pointR=pointsR.available() ;
fhcrcmars 20:f541b6b063fa 192 int correct_pointR_number=0;
fhcrcmars 20:f541b6b063fa 193 n_pointL=pointsL.available() ;
fhcrcmars 20:f541b6b063fa 194 int correct_pointL_number=0;
backman 19:4869b10a962e 195
fhcrcmars 21:4e8a4f66aaef 196 pc.printf("R: ");
fhcrcmars 21:4e8a4f66aaef 197
fhcrcmars 20:f541b6b063fa 198 for(int i=0;i<n_pointR;i++){
fhcrcmars 20:f541b6b063fa 199 pointsR.pop(&point);
fhcrcmars 21:4e8a4f66aaef 200 pc.printf("%d ",point);
backman 18:eb675df59c7f 201 //algorithm
fhcrcmars 20:f541b6b063fa 202 if(point>0){
fhcrcmars 20:f541b6b063fa 203 sum_error_R+=point; //because R's black is on the right side of line
fhcrcmars 20:f541b6b063fa 204 correct_pointR_number++;
fhcrcmars 20:f541b6b063fa 205 }
backman 18:eb675df59c7f 206 }
fhcrcmars 21:4e8a4f66aaef 207
fhcrcmars 21:4e8a4f66aaef 208 pc.printf("L:");
fhcrcmars 20:f541b6b063fa 209 for(int i=0;i<n_pointL;i++){
fhcrcmars 21:4e8a4f66aaef 210 pointsL.pop(&point);
fhcrcmars 21:4e8a4f66aaef 211 pc.printf("%d ",point);
fhcrcmars 20:f541b6b063fa 212 if(point>0){
fhcrcmars 20:f541b6b063fa 213 sum_error_L+=point; //because R's black is on the right side of line
fhcrcmars 20:f541b6b063fa 214 correct_pointL_number++;
fhcrcmars 20:f541b6b063fa 215 }
fhcrcmars 20:f541b6b063fa 216 }
fhcrcmars 21:4e8a4f66aaef 217 pc.printf("\r\n");
fhcrcmars 20:f541b6b063fa 218 int error_R_ave=(correct_pointR_number==0)?0:sum_error_R/correct_pointR_number;
fhcrcmars 20:f541b6b063fa 219 int error_L_ave=(correct_pointL_number==0)?0:sum_error_L/correct_pointL_number;
fhcrcmars 20:f541b6b063fa 220
fhcrcmars 21:4e8a4f66aaef 221 // pc.printf("L: %d R: %d\r\n",error_L_ave,error_R_ave);
fhcrcmars 21:4e8a4f66aaef 222
fhcrcmars 21:4e8a4f66aaef 223
fhcrcmars 20:f541b6b063fa 224 //two line
fhcrcmars 20:f541b6b063fa 225 if(error_L_ave!=0 && error_R_ave!=0){
fhcrcmars 20:f541b6b063fa 226 servo.set_angle(cam_to_M_ctrlr.compute((error_L_ave+error_R_ave)/2,center) );
fhcrcmars 20:f541b6b063fa 227 }
fhcrcmars 20:f541b6b063fa 228
fhcrcmars 20:f541b6b063fa 229 //in the correct think, one line should not appear
fhcrcmars 20:f541b6b063fa 230 //right line
fhcrcmars 20:f541b6b063fa 231 else if(error_L_ave==0 && error_R_ave!=0){
fhcrcmars 21:4e8a4f66aaef 232 servo.set_angle(cam_to_M_ctrlr.compute(error_R_ave,30));
fhcrcmars 20:f541b6b063fa 233 }
fhcrcmars 20:f541b6b063fa 234 //left line
fhcrcmars 20:f541b6b063fa 235 else if(error_R_ave==0 && error_L_ave!=0){
fhcrcmars 21:4e8a4f66aaef 236 servo.set_angle(cam_to_M_ctrlr.compute(error_L_ave,88));
fhcrcmars 20:f541b6b063fa 237 }
fhcrcmars 20:f541b6b063fa 238 //no line
fhcrcmars 20:f541b6b063fa 239 else if(error_L_ave!=0 && error_R_ave!=0){}
backman 19:4869b10a962e 240
backman 19:4869b10a962e 241
fhcrcmars 20:f541b6b063fa 242 // cam_to_M_ctrlr.compute(black_center+sum_error_R/n_pointR,center);
fhcrcmars 20:f541b6b063fa 243
fhcrcmars 20:f541b6b063fa 244 // servo.set_angle( cam_to_M_ctrlr.compute(black_center+sum_e/n_pointR,center) );
backman 19:4869b10a962e 245
backman 19:4869b10a962e 246 // pc.printf("dfdf");
backman 18:eb675df59c7f 247 //if(b_r_c!=-1)
backman 19:4869b10a962e 248 // v_servo=cam_to_M_ctrlr.compute(b_r_c,R_target);
backman 18:eb675df59c7f 249 //if(b_l_c!=-1)
backman 18:eb675df59c7f 250 // v_servo=cam_to_M_ctrlr.compute(b_l_c,L_target);
backman 16:b78dce5c0e98 251
backman 18:eb675df59c7f 252
backman 15:585df3979be8 253 // v_servo=pot2.read();
backman 19:4869b10a962e 254
backman 18:eb675df59c7f 255
backman 18:eb675df59c7f 256 #ifdef task_ma_time
backman 18:eb675df59c7f 257 servo_p=0;
backman 18:eb675df59c7f 258 #endif
backman 18:eb675df59c7f 259
backman 15:585df3979be8 260
backman 19:4869b10a962e 261 Thread::wait(20);
backman 19:4869b10a962e 262
backman 19:4869b10a962e 263
backman 11:418e39749f48 264 }
backman 11:418e39749f48 265
backman 11:418e39749f48 266
backman 11:418e39749f48 267
backman 11:418e39749f48 268 }
backman 11:418e39749f48 269
backman 15:585df3979be8 270
backman 15:585df3979be8 271
backman 15:585df3979be8 272
backman 15:585df3979be8 273
backman 15:585df3979be8 274
backman 15:585df3979be8 275
backman 11:418e39749f48 276 void motor_thread(void const *args){
backman 11:418e39749f48 277
backman 11:418e39749f48 278 while(1){
backman 7:fd976e1ced33 279
backman 14:2d90b0066fc6 280 v_motor=pot1.read();
backman 11:418e39749f48 281 MotorA.rotate(v_motor);
backman 11:418e39749f48 282 MotorB.rotate(v_motor);
backman 7:fd976e1ced33 283
backman 14:2d90b0066fc6 284 Thread::wait(10);
backman 11:418e39749f48 285 }
backman 11:418e39749f48 286
backman 11:418e39749f48 287
backman 7:fd976e1ced33 288
backman 11:418e39749f48 289
backman 11:418e39749f48 290 }
backman 11:418e39749f48 291
backman 11:418e39749f48 292
backman 11:418e39749f48 293
backman 11:418e39749f48 294
backman 11:418e39749f48 295
backman 11:418e39749f48 296
backman 11:418e39749f48 297
backman 11:418e39749f48 298
backman 11:418e39749f48 299
backman 11:418e39749f48 300 int main() {
backman 11:418e39749f48 301
backman 11:418e39749f48 302 // baud rate init --- no function
backman 12:fdada4af384a 303
backman 15:585df3979be8 304 servo.set_angle(0.055);
backman 12:fdada4af384a 305 pc.baud(115200);
backman 19:4869b10a962e 306
backman 19:4869b10a962e 307
backman 19:4869b10a962e 308
backman 19:4869b10a962e 309 //while(1);
backman 19:4869b10a962e 310
backman 19:4869b10a962e 311
backman 19:4869b10a962e 312
backman 19:4869b10a962e 313
backman 19:4869b10a962e 314 /* while(1){
backman 8:8e49e21d80a2 315
backman 11:418e39749f48 316 if(tsi.readPercentage()>0.00011)
backman 11:418e39749f48 317 break;
backman 11:418e39749f48 318 }
backman 19:4869b10a962e 319
backman 19:4869b10a962e 320 */
backman 15:585df3979be8 321
backman 15:585df3979be8 322
backman 15:585df3979be8 323
backman 12:fdada4af384a 324 Thread th_c(cam_thread);
backman 11:418e39749f48 325 // Thread thread(ctrl_thread);
fhcrcmars 21:4e8a4f66aaef 326 Thread th_s(servo_thread);
fhcrcmars 21:4e8a4f66aaef 327 Thread th_m(motor_thread);
fhcrcmars 21:4e8a4f66aaef 328 // Thread th_de(de_thread);
backman 19:4869b10a962e 329
backman 19:4869b10a962e 330 //Thread dddd(pin2_thread);
backman 11:418e39749f48 331 while(1){
backman 10:03d5aa2511c4 332
backman 19:4869b10a962e 333 #ifdef task_ma_time
backman 19:4869b10a962e 334 idle_p=1;
backman 19:4869b10a962e 335 #endif
backman 19:4869b10a962e 336
backman 19:4869b10a962e 337
backman 19:4869b10a962e 338
backman 19:4869b10a962e 339 #ifdef task_ma_time
backman 19:4869b10a962e 340 idle_p=0;
backman 19:4869b10a962e 341 #endif
backman 19:4869b10a962e 342
backman 11:418e39749f48 343
backman 11:418e39749f48 344 //idle
backman 12:fdada4af384a 345 // stdio_mutex.lock();
backman 12:fdada4af384a 346 // printf("L: %d mid: %d R: %d\r\n",line3[0],line3[1],line3[2]);
backman 11:418e39749f48 347 // stdio_mutex.unlock();
fhcrcmars 20:f541b6b063fa 348 // Thread::wait(1000);
backman 12:fdada4af384a 349
backman 7:fd976e1ced33 350 }
backman 7:fd976e1ced33 351
backman 6:5a39bde2e016 352
backman 6:5a39bde2e016 353
backman 7:fd976e1ced33 354
backman 0:68c173249c01 355
backman 0:68c173249c01 356
backman 0:68c173249c01 357
backman 1:82bc25a7b68b 358 return 0;
backman 0:68c173249c01 359
backman 0:68c173249c01 360
backman 0:68c173249c01 361 }