kao yi / Mbed 2 deprecated Boboobooov5

wu

Dependencies:   mbed-rtos mbed

Fork of Boboobooov4 by kao yi

main.cpp@20:30799cbda86b, 2014-07-02 (annotated)

Committer:
backman
Date:
Wed Jul 02 06:44:48 2014 +0000
Revision:
20:30799cbda86b
Parent:
19:4869b10a962e 
al

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