kao yi
/
Boboobooov5
wu
Fork of Boboobooov4 by
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
- Child:
- 20:30799cbda86b
wang
Who changed what in which revision?
User | Revision | Line number | New 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 | } |