kao yi / Mbed 2 deprecated Boboobooov5

wu

Dependencies:   mbed-rtos mbed

Fork of Boboobooov4 by kao yi

main.cpp@18:eb675df59c7f, 2014-06-30 (annotated)

Committer:
backman
Date:
Mon Jun 30 07:01:58 2014 +0000
Revision:
18:eb675df59c7f
Parent:
17:3dac99cf2b89
Child:
19:4869b10a962e
task_de

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 17:3dac99cf2b89 12 #define L_eye
backman 17:3dac99cf2b89 13 //#define R_eye
backman 7:fd976e1ced33 14 #define motor_on
backman 6:5a39bde2e016 15 #define Pcontroller
backman 8:8e49e21d80a2 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 15:585df3979be8 24 #define t_cam 2
backman 11:418e39749f48 25
backman 11:418e39749f48 26
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 18:eb675df59c7f 47 DigitalOut cam_p(PTD1); //cam black
backman 18:eb675df59c7f 48 DigitalOut servo_p(PTA13); //servo coffee
backman 18:eb675df59c7f 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
backman 18:eb675df59c7f 60 Stack<int> points(10);
backman 18:eb675df59c7f 61
backman 18:eb675df59c7f 62
backman 18:eb675df59c7f 63
backman 14:2d90b0066fc6 64
backman 14:2d90b0066fc6 65
backman 11:418e39749f48 66
backman 15:585df3979be8 67 static int b_r_c=64;
backman 16:b78dce5c0e98 68 static int b_l_c=64;
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 18:eb675df59c7f 83 #endif
backman 15:585df3979be8 84 cam.read();
backman 11:418e39749f48 85
backman 15:585df3979be8 86
backman 15:585df3979be8 87 b_r_c=cam.black_centerR();
backman 18:eb675df59c7f 88 // b_l_c=cam.black_centerL();
backman 18:eb675df59c7f 89
backman 18:eb675df59c7f 90 points.push(b_r_c);
backman 18:eb675df59c7f 91
backman 18:eb675df59c7f 92
backman 18:eb675df59c7f 93 #ifdef task_ma_time
backman 18:eb675df59c7f 94 cam_p=0;
backman 18:eb675df59c7f 95 #endif
backman 18:eb675df59c7f 96
backman 18:eb675df59c7f 97
backman 15:585df3979be8 98 Thread::wait(t_cam);
backman 11:418e39749f48 99
backman 11:418e39749f48 100 }
backman 11:418e39749f48 101
backman 11:418e39749f48 102 }
backman 11:418e39749f48 103 // function
backman 11:418e39749f48 104 void de_thread(void const *args){
backman 9:33b99cb45e99 105
backman 11:418e39749f48 106 while(1){
backman 11:418e39749f48 107
backman 18:eb675df59c7f 108
backman 18:eb675df59c7f 109 #ifdef task_ma_time
backman 18:eb675df59c7f 110 de_p=0;
backman 18:eb675df59c7f 111 #endif
backman 11:418e39749f48 112
backman 11:418e39749f48 113
backman 11:418e39749f48 114 stdio_mutex.lock();
backman 11:418e39749f48 115 #ifdef Debug_cam_uart
backman 11:418e39749f48 116 #ifdef L_eye
backman 17:3dac99cf2b89 117 pc.printf("L: ");
backman 13:63f9a5101205 118 for(int i=128;i>=0;i--){
backman 11:418e39749f48 119 if(i==64)
backman 12:fdada4af384a 120 pc.printf("X");
backman 11:418e39749f48 121 else if(i<10)
backman 12:fdada4af384a 122 pc.printf("-");
backman 11:418e39749f48 123 else if(i>117)
backman 13:63f9a5101205 124 pc.printf("-");
backman 11:418e39749f48 125 else
backman 13:63f9a5101205 126 pc.printf("%c", cam.sign_line_imageL[i]);
backman 11:418e39749f48 127 }
backman 13:63f9a5101205 128
backman 12:fdada4af384a 129 pc.printf(" || ");
backman 11:418e39749f48 130 #endif
backman 11:418e39749f48 131 #ifdef R_eye
backman 12:fdada4af384a 132 pc.printf("R: ");
backman 11:418e39749f48 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 12:fdada4af384a 139 pc.printf("-");
backman 11:418e39749f48 140 else
backman 12:fdada4af384a 141 pc.printf("%c", cam.sign_line_imageR[i]);
backman 11:418e39749f48 142 }
backman 17:3dac99cf2b89 143 #endif
backman 17:3dac99cf2b89 144
backman 17:3dac99cf2b89 145 pc.printf("\r\n Rcenter :%d Lcenter : %d servo: %f \r\n",cam.black_centerR(),cam.black_centerL(),v_servo);
backman 17:3dac99cf2b89 146
backman 15:585df3979be8 147
backman 11:418e39749f48 148 stdio_mutex.unlock();
backman 1:82bc25a7b68b 149
backman 1:82bc25a7b68b 150
backman 18:eb675df59c7f 151
backman 18:eb675df59c7f 152 #ifdef task_ma_time
backman 18:eb675df59c7f 153 de_p=1;
backman 18:eb675df59c7f 154 #endif
backman 11:418e39749f48 155
backman 7:fd976e1ced33 156
backman 7:fd976e1ced33 157
backman 11:418e39749f48 158 #endif
backman 11:418e39749f48 159 Thread::wait(1);
backman 7:fd976e1ced33 160
backman 11:418e39749f48 161
backman 11:418e39749f48 162 }
backman 11:418e39749f48 163
backman 11:418e39749f48 164 }
backman 11:418e39749f48 165
backman 11:418e39749f48 166
backman 7:fd976e1ced33 167
backman 7:fd976e1ced33 168
backman 11:418e39749f48 169 void servo_thread(void const *args){
backman 11:418e39749f48 170
backman 11:418e39749f48 171
backman 11:418e39749f48 172 while(1){
backman 18:eb675df59c7f 173
backman 18:eb675df59c7f 174 #ifdef task_ma_time
backman 18:eb675df59c7f 175 servo_p=1;
backman 18:eb675df59c7f 176 #endif
backman 15:585df3979be8 177
backman 18:eb675df59c7f 178 int point;
backman 18:eb675df59c7f 179
backman 18:eb675df59c7f 180 for(;points.available()!=0;){
backman 18:eb675df59c7f 181 points.pop(&point);
backman 18:eb675df59c7f 182
backman 18:eb675df59c7f 183 //algorithm
backman 18:eb675df59c7f 184
backman 18:eb675df59c7f 185 }
backman 18:eb675df59c7f 186
backman 18:eb675df59c7f 187 //if(b_r_c!=-1)
backman 16:b78dce5c0e98 188 v_servo=cam_to_M_ctrlr.compute(b_r_c,R_target);
backman 18:eb675df59c7f 189 //if(b_l_c!=-1)
backman 18:eb675df59c7f 190 // v_servo=cam_to_M_ctrlr.compute(b_l_c,L_target);
backman 16:b78dce5c0e98 191
backman 18:eb675df59c7f 192
backman 15:585df3979be8 193 // v_servo=pot2.read();
backman 18:eb675df59c7f 194 servo.set_angle(v_servo);
backman 18:eb675df59c7f 195
backman 18:eb675df59c7f 196 #ifdef task_ma_time
backman 18:eb675df59c7f 197 servo_p=0;
backman 18:eb675df59c7f 198 #endif
backman 18:eb675df59c7f 199
backman 15:585df3979be8 200
backman 11:418e39749f48 201 Thread::wait(20);
backman 11:418e39749f48 202 }
backman 11:418e39749f48 203
backman 11:418e39749f48 204
backman 11:418e39749f48 205
backman 11:418e39749f48 206 }
backman 11:418e39749f48 207
backman 15:585df3979be8 208
backman 15:585df3979be8 209
backman 15:585df3979be8 210
backman 15:585df3979be8 211
backman 15:585df3979be8 212
backman 15:585df3979be8 213
backman 11:418e39749f48 214 void motor_thread(void const *args){
backman 11:418e39749f48 215
backman 11:418e39749f48 216 while(1){
backman 7:fd976e1ced33 217
backman 14:2d90b0066fc6 218 v_motor=pot1.read();
backman 11:418e39749f48 219 MotorA.rotate(v_motor);
backman 11:418e39749f48 220 MotorB.rotate(v_motor);
backman 7:fd976e1ced33 221
backman 14:2d90b0066fc6 222 Thread::wait(10);
backman 11:418e39749f48 223 }
backman 11:418e39749f48 224
backman 11:418e39749f48 225
backman 7:fd976e1ced33 226
backman 11:418e39749f48 227
backman 11:418e39749f48 228 }
backman 11:418e39749f48 229
backman 11:418e39749f48 230
backman 11:418e39749f48 231
backman 11:418e39749f48 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 int main() {
backman 11:418e39749f48 239
backman 11:418e39749f48 240 // baud rate init --- no function
backman 12:fdada4af384a 241
backman 15:585df3979be8 242 servo.set_angle(0.055);
backman 12:fdada4af384a 243 pc.baud(115200);
backman 11:418e39749f48 244 /*
backman 11:418e39749f48 245 while(1){
backman 8:8e49e21d80a2 246
backman 11:418e39749f48 247 if(tsi.readPercentage()>0.00011)
backman 11:418e39749f48 248 break;
backman 11:418e39749f48 249 }
backman 11:418e39749f48 250 */
backman 15:585df3979be8 251
backman 15:585df3979be8 252
backman 15:585df3979be8 253
backman 15:585df3979be8 254
backman 12:fdada4af384a 255 Thread th_c(cam_thread);
backman 11:418e39749f48 256 // Thread thread(ctrl_thread);
backman 14:2d90b0066fc6 257 Thread th_s(servo_thread);
backman 18:eb675df59c7f 258 // Thread th_m(motor_thread);
backman 18:eb675df59c7f 259 // Thread th_de(de_thread);
backman 11:418e39749f48 260 while(1){
backman 10:03d5aa2511c4 261
backman 11:418e39749f48 262
backman 11:418e39749f48 263 //idle
backman 12:fdada4af384a 264 // stdio_mutex.lock();
backman 12:fdada4af384a 265 // printf("L: %d mid: %d R: %d\r\n",line3[0],line3[1],line3[2]);
backman 11:418e39749f48 266 // stdio_mutex.unlock();
backman 14:2d90b0066fc6 267 // Thread::wait(2000);
backman 12:fdada4af384a 268
backman 7:fd976e1ced33 269 }
backman 7:fd976e1ced33 270
backman 6:5a39bde2e016 271
backman 6:5a39bde2e016 272
backman 7:fd976e1ced33 273
backman 0:68c173249c01 274
backman 0:68c173249c01 275
backman 0:68c173249c01 276
backman 1:82bc25a7b68b 277 return 0;
backman 0:68c173249c01 278
backman 0:68c173249c01 279
backman 0:68c173249c01 280 }