kao yi / Mbed 2 deprecated Boboobooov4

wu

Dependencies:   mbed-rtos mbed

Fork of Bov3 by kao yi

main.cpp@19:4869b10a962e, 2014-07-02 (annotated)

Committer:
backman
Date:
Wed Jul 02 03:23:07 2014 +0000
Revision:
19:4869b10a962e
Parent:
18:eb675df59c7f 
wang

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 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
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 pc.printf("stack n: %d",points.available());
backman 19:4869b10a962e 164
backman 11:418e39749f48 165 stdio_mutex.unlock();
backman 1:82bc25a7b68b 166
backman 1:82bc25a7b68b 167
backman 18:eb675df59c7f 168
backman 18:eb675df59c7f 169 #ifdef task_ma_time
backman 19:4869b10a962e 170 //de_p=1;
backman 18:eb675df59c7f 171 #endif
backman 11:418e39749f48 172
backman 7:fd976e1ced33 173
backman 7:fd976e1ced33 174
backman 11:418e39749f48 175 #endif
backman 19:4869b10a962e 176 Thread::wait(10);
backman 7:fd976e1ced33 177
backman 11:418e39749f48 178
backman 11:418e39749f48 179 }
backman 11:418e39749f48 180
backman 11:418e39749f48 181 }
backman 11:418e39749f48 182
backman 11:418e39749f48 183
backman 7:fd976e1ced33 184
backman 7:fd976e1ced33 185
backman 11:418e39749f48 186 void servo_thread(void const *args){
backman 11:418e39749f48 187
backman 11:418e39749f48 188
backman 11:418e39749f48 189 while(1){
backman 18:eb675df59c7f 190
backman 18:eb675df59c7f 191 #ifdef task_ma_time
backman 18:eb675df59c7f 192 servo_p=1;
backman 18:eb675df59c7f 193 #endif
backman 15:585df3979be8 194
backman 19:4869b10a962e 195 int point;
backman 19:4869b10a962e 196 int sum_e=0;
backman 19:4869b10a962e 197 int n_point=0;
backman 19:4869b10a962e 198 n_point=points.available() ;
backman 19:4869b10a962e 199
backman 19:4869b10a962e 200 for(int i=0;i<n_point;i++){
backman 19:4869b10a962e 201 points.pop(&point);
backman 18:eb675df59c7f 202 //algorithm
backman 19:4869b10a962e 203
backman 19:4869b10a962e 204 sum_e+=point-black_center;
backman 19:4869b10a962e 205
backman 18:eb675df59c7f 206 }
backman 19:4869b10a962e 207 printf("error :%d compute: %f\r\n",sum_e/n_point, cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target));
backman 19:4869b10a962e 208
backman 19:4869b10a962e 209
backman 19:4869b10a962e 210 cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target);
backman 19:4869b10a962e 211 // pc.printf("expect :%d \r\n",black_center+sum_e/n_point );
backman 19:4869b10a962e 212 // pc.printf("angle :%f", cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target) );
backman 19:4869b10a962e 213 servo.set_angle( cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target) );
backman 19:4869b10a962e 214
backman 19:4869b10a962e 215 // pc.printf("dfdf");
backman 18:eb675df59c7f 216 //if(b_r_c!=-1)
backman 19:4869b10a962e 217 // v_servo=cam_to_M_ctrlr.compute(b_r_c,R_target);
backman 18:eb675df59c7f 218 //if(b_l_c!=-1)
backman 18:eb675df59c7f 219 // v_servo=cam_to_M_ctrlr.compute(b_l_c,L_target);
backman 16:b78dce5c0e98 220
backman 18:eb675df59c7f 221
backman 15:585df3979be8 222 // v_servo=pot2.read();
backman 19:4869b10a962e 223
backman 18:eb675df59c7f 224
backman 18:eb675df59c7f 225 #ifdef task_ma_time
backman 18:eb675df59c7f 226 servo_p=0;
backman 18:eb675df59c7f 227 #endif
backman 18:eb675df59c7f 228
backman 15:585df3979be8 229
backman 19:4869b10a962e 230 Thread::wait(20);
backman 19:4869b10a962e 231
backman 19:4869b10a962e 232
backman 11:418e39749f48 233 }
backman 11:418e39749f48 234
backman 11:418e39749f48 235
backman 11:418e39749f48 236
backman 11:418e39749f48 237 }
backman 11:418e39749f48 238
backman 15:585df3979be8 239
backman 15:585df3979be8 240
backman 15:585df3979be8 241
backman 15:585df3979be8 242
backman 15:585df3979be8 243
backman 15:585df3979be8 244
backman 11:418e39749f48 245 void motor_thread(void const *args){
backman 11:418e39749f48 246
backman 11:418e39749f48 247 while(1){
backman 7:fd976e1ced33 248
backman 14:2d90b0066fc6 249 v_motor=pot1.read();
backman 11:418e39749f48 250 MotorA.rotate(v_motor);
backman 11:418e39749f48 251 MotorB.rotate(v_motor);
backman 7:fd976e1ced33 252
backman 14:2d90b0066fc6 253 Thread::wait(10);
backman 11:418e39749f48 254 }
backman 11:418e39749f48 255
backman 11:418e39749f48 256
backman 7:fd976e1ced33 257
backman 11:418e39749f48 258
backman 11:418e39749f48 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 int main() {
backman 11:418e39749f48 270
backman 11:418e39749f48 271 // baud rate init --- no function
backman 12:fdada4af384a 272
backman 15:585df3979be8 273 servo.set_angle(0.055);
backman 12:fdada4af384a 274 pc.baud(115200);
backman 19:4869b10a962e 275
backman 19:4869b10a962e 276
backman 19:4869b10a962e 277
backman 19:4869b10a962e 278 //while(1);
backman 19:4869b10a962e 279
backman 19:4869b10a962e 280
backman 19:4869b10a962e 281
backman 19:4869b10a962e 282
backman 19:4869b10a962e 283 /* while(1){
backman 8:8e49e21d80a2 284
backman 11:418e39749f48 285 if(tsi.readPercentage()>0.00011)
backman 11:418e39749f48 286 break;
backman 11:418e39749f48 287 }
backman 19:4869b10a962e 288
backman 19:4869b10a962e 289 */
backman 15:585df3979be8 290
backman 15:585df3979be8 291
backman 15:585df3979be8 292
backman 12:fdada4af384a 293 Thread th_c(cam_thread);
backman 11:418e39749f48 294 // Thread thread(ctrl_thread);
backman 14:2d90b0066fc6 295 Thread th_s(servo_thread);
backman 18:eb675df59c7f 296 // Thread th_m(motor_thread);
backman 19:4869b10a962e 297 // Thread th_de(de_thread);
backman 19:4869b10a962e 298
backman 19:4869b10a962e 299 //Thread dddd(pin2_thread);
backman 11:418e39749f48 300 while(1){
backman 10:03d5aa2511c4 301
backman 19:4869b10a962e 302 #ifdef task_ma_time
backman 19:4869b10a962e 303 idle_p=1;
backman 19:4869b10a962e 304 #endif
backman 19:4869b10a962e 305
backman 19:4869b10a962e 306
backman 19:4869b10a962e 307
backman 19:4869b10a962e 308 #ifdef task_ma_time
backman 19:4869b10a962e 309 idle_p=0;
backman 19:4869b10a962e 310 #endif
backman 19:4869b10a962e 311
backman 11:418e39749f48 312
backman 11:418e39749f48 313 //idle
backman 12:fdada4af384a 314 // stdio_mutex.lock();
backman 12:fdada4af384a 315 // printf("L: %d mid: %d R: %d\r\n",line3[0],line3[1],line3[2]);
backman 11:418e39749f48 316 // stdio_mutex.unlock();
backman 19:4869b10a962e 317 Thread::wait(1000);
backman 12:fdada4af384a 318
backman 7:fd976e1ced33 319 }
backman 7:fd976e1ced33 320
backman 6:5a39bde2e016 321
backman 6:5a39bde2e016 322
backman 7:fd976e1ced33 323
backman 0:68c173249c01 324
backman 0:68c173249c01 325
backman 0:68c173249c01 326
backman 1:82bc25a7b68b 327 return 0;
backman 0:68c173249c01 328
backman 0:68c173249c01 329
backman 0:68c173249c01 330 }