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@20:f541b6b063fa, 2014-07-09 (annotated)

Committer:
fhcrcmars
Date:
Wed Jul 09 12:48:46 2014 +0000
Revision:
20:f541b6b063fa
Parent:
19:4869b10a962e
Child:
21:4e8a4f66aaef
tet

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 20:f541b6b063fa 97 //printf("push :%d\r\n",b_r_c);
fhcrcmars 20:f541b6b063fa 98 pointsR.push(b_r_c);
fhcrcmars 20:f541b6b063fa 99 pointsR.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
backman 19:4869b10a962e 185 int point;
fhcrcmars 20:f541b6b063fa 186 int sum_error_R=0;
fhcrcmars 20:f541b6b063fa 187 int sum_error_L=0;
fhcrcmars 20:f541b6b063fa 188 int n_pointR=0;
fhcrcmars 20:f541b6b063fa 189 int n_pointL=0;
fhcrcmars 20:f541b6b063fa 190 n_pointR=pointsR.available() ;
fhcrcmars 20:f541b6b063fa 191 int correct_pointR_number=0;
fhcrcmars 20:f541b6b063fa 192 n_pointL=pointsL.available() ;
fhcrcmars 20:f541b6b063fa 193 int correct_pointL_number=0;
backman 19:4869b10a962e 194
fhcrcmars 20:f541b6b063fa 195 for(int i=0;i<n_pointR;i++){
fhcrcmars 20:f541b6b063fa 196 pointsR.pop(&point);
backman 18:eb675df59c7f 197 //algorithm
fhcrcmars 20:f541b6b063fa 198 if(point>0){
fhcrcmars 20:f541b6b063fa 199 sum_error_R+=point; //because R's black is on the right side of line
fhcrcmars 20:f541b6b063fa 200 correct_pointR_number++;
fhcrcmars 20:f541b6b063fa 201 }
backman 18:eb675df59c7f 202 }
fhcrcmars 20:f541b6b063fa 203 for(int i=0;i<n_pointL;i++){
fhcrcmars 20:f541b6b063fa 204 if(point>0){
fhcrcmars 20:f541b6b063fa 205 sum_error_L+=point; //because R's black is on the right side of line
fhcrcmars 20:f541b6b063fa 206 correct_pointL_number++;
fhcrcmars 20:f541b6b063fa 207 }
fhcrcmars 20:f541b6b063fa 208 }
fhcrcmars 20:f541b6b063fa 209
fhcrcmars 20:f541b6b063fa 210 int error_R_ave=(correct_pointR_number==0)?0:sum_error_R/correct_pointR_number;
fhcrcmars 20:f541b6b063fa 211 int error_L_ave=(correct_pointL_number==0)?0:sum_error_L/correct_pointL_number;
fhcrcmars 20:f541b6b063fa 212
fhcrcmars 20:f541b6b063fa 213 //two line
fhcrcmars 20:f541b6b063fa 214 if(error_L_ave!=0 && error_R_ave!=0){
fhcrcmars 20:f541b6b063fa 215 servo.set_angle(cam_to_M_ctrlr.compute((error_L_ave+error_R_ave)/2,center) );
fhcrcmars 20:f541b6b063fa 216 }
fhcrcmars 20:f541b6b063fa 217
fhcrcmars 20:f541b6b063fa 218 //in the correct think, one line should not appear
fhcrcmars 20:f541b6b063fa 219 //right line
fhcrcmars 20:f541b6b063fa 220 else if(error_L_ave==0 && error_R_ave!=0){
fhcrcmars 20:f541b6b063fa 221 }
fhcrcmars 20:f541b6b063fa 222 //left line
fhcrcmars 20:f541b6b063fa 223 else if(error_R_ave==0 && error_L_ave!=0){
fhcrcmars 20:f541b6b063fa 224 }
fhcrcmars 20:f541b6b063fa 225 //no line
fhcrcmars 20:f541b6b063fa 226 else if(error_L_ave!=0 && error_R_ave!=0){}
backman 19:4869b10a962e 227
backman 19:4869b10a962e 228
fhcrcmars 20:f541b6b063fa 229 // cam_to_M_ctrlr.compute(black_center+sum_error_R/n_pointR,center);
fhcrcmars 20:f541b6b063fa 230
fhcrcmars 20:f541b6b063fa 231 // servo.set_angle( cam_to_M_ctrlr.compute(black_center+sum_e/n_pointR,center) );
backman 19:4869b10a962e 232
backman 19:4869b10a962e 233 // pc.printf("dfdf");
backman 18:eb675df59c7f 234 //if(b_r_c!=-1)
backman 19:4869b10a962e 235 // v_servo=cam_to_M_ctrlr.compute(b_r_c,R_target);
backman 18:eb675df59c7f 236 //if(b_l_c!=-1)
backman 18:eb675df59c7f 237 // v_servo=cam_to_M_ctrlr.compute(b_l_c,L_target);
backman 16:b78dce5c0e98 238
backman 18:eb675df59c7f 239
backman 15:585df3979be8 240 // v_servo=pot2.read();
backman 19:4869b10a962e 241
backman 18:eb675df59c7f 242
backman 18:eb675df59c7f 243 #ifdef task_ma_time
backman 18:eb675df59c7f 244 servo_p=0;
backman 18:eb675df59c7f 245 #endif
backman 18:eb675df59c7f 246
backman 15:585df3979be8 247
backman 19:4869b10a962e 248 Thread::wait(20);
backman 19:4869b10a962e 249
backman 19:4869b10a962e 250
backman 11:418e39749f48 251 }
backman 11:418e39749f48 252
backman 11:418e39749f48 253
backman 11:418e39749f48 254
backman 11:418e39749f48 255 }
backman 11:418e39749f48 256
backman 15:585df3979be8 257
backman 15:585df3979be8 258
backman 15:585df3979be8 259
backman 15:585df3979be8 260
backman 15:585df3979be8 261
backman 15:585df3979be8 262
backman 11:418e39749f48 263 void motor_thread(void const *args){
backman 11:418e39749f48 264
backman 11:418e39749f48 265 while(1){
backman 7:fd976e1ced33 266
backman 14:2d90b0066fc6 267 v_motor=pot1.read();
backman 11:418e39749f48 268 MotorA.rotate(v_motor);
backman 11:418e39749f48 269 MotorB.rotate(v_motor);
backman 7:fd976e1ced33 270
backman 14:2d90b0066fc6 271 Thread::wait(10);
backman 11:418e39749f48 272 }
backman 11:418e39749f48 273
backman 11:418e39749f48 274
backman 7:fd976e1ced33 275
backman 11:418e39749f48 276
backman 11:418e39749f48 277 }
backman 11:418e39749f48 278
backman 11:418e39749f48 279
backman 11:418e39749f48 280
backman 11:418e39749f48 281
backman 11:418e39749f48 282
backman 11:418e39749f48 283
backman 11:418e39749f48 284
backman 11:418e39749f48 285
backman 11:418e39749f48 286
backman 11:418e39749f48 287 int main() {
backman 11:418e39749f48 288
backman 11:418e39749f48 289 // baud rate init --- no function
backman 12:fdada4af384a 290
backman 15:585df3979be8 291 servo.set_angle(0.055);
backman 12:fdada4af384a 292 pc.baud(115200);
backman 19:4869b10a962e 293
backman 19:4869b10a962e 294
backman 19:4869b10a962e 295
backman 19:4869b10a962e 296 //while(1);
backman 19:4869b10a962e 297
backman 19:4869b10a962e 298
backman 19:4869b10a962e 299
backman 19:4869b10a962e 300
backman 19:4869b10a962e 301 /* while(1){
backman 8:8e49e21d80a2 302
backman 11:418e39749f48 303 if(tsi.readPercentage()>0.00011)
backman 11:418e39749f48 304 break;
backman 11:418e39749f48 305 }
backman 19:4869b10a962e 306
backman 19:4869b10a962e 307 */
backman 15:585df3979be8 308
backman 15:585df3979be8 309
backman 15:585df3979be8 310
backman 12:fdada4af384a 311 Thread th_c(cam_thread);
backman 11:418e39749f48 312 // Thread thread(ctrl_thread);
fhcrcmars 20:f541b6b063fa 313 // Thread th_s(servo_thread);
fhcrcmars 20:f541b6b063fa 314 // Thread th_m(motor_thread);
fhcrcmars 20:f541b6b063fa 315 Thread th_de(de_thread);
backman 19:4869b10a962e 316
backman 19:4869b10a962e 317 //Thread dddd(pin2_thread);
backman 11:418e39749f48 318 while(1){
backman 10:03d5aa2511c4 319
backman 19:4869b10a962e 320 #ifdef task_ma_time
backman 19:4869b10a962e 321 idle_p=1;
backman 19:4869b10a962e 322 #endif
backman 19:4869b10a962e 323
backman 19:4869b10a962e 324
backman 19:4869b10a962e 325
backman 19:4869b10a962e 326 #ifdef task_ma_time
backman 19:4869b10a962e 327 idle_p=0;
backman 19:4869b10a962e 328 #endif
backman 19:4869b10a962e 329
backman 11:418e39749f48 330
backman 11:418e39749f48 331 //idle
backman 12:fdada4af384a 332 // stdio_mutex.lock();
backman 12:fdada4af384a 333 // printf("L: %d mid: %d R: %d\r\n",line3[0],line3[1],line3[2]);
backman 11:418e39749f48 334 // stdio_mutex.unlock();
fhcrcmars 20:f541b6b063fa 335 // Thread::wait(1000);
backman 12:fdada4af384a 336
backman 7:fd976e1ced33 337 }
backman 7:fd976e1ced33 338
backman 6:5a39bde2e016 339
backman 6:5a39bde2e016 340
backman 7:fd976e1ced33 341
backman 0:68c173249c01 342
backman 0:68c173249c01 343
backman 0:68c173249c01 344
backman 1:82bc25a7b68b 345 return 0;
backman 0:68c173249c01 346
backman 0:68c173249c01 347
backman 0:68c173249c01 348 }