kao yi / Mbed 2 deprecated Boboobooov5

wu

Dependencies:   mbed-rtos mbed

Fork of Boboobooov4 by kao yi

main.cpp@14:2d90b0066fc6, 2014-06-28 (annotated)

Committer:
backman
Date:
Sat Jun 28 12:16:33 2014 +0000
Revision:
14:2d90b0066fc6
Parent:
13:63f9a5101205
Child:
15:585df3979be8
v4-thread

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 3:c5f2281b3ed2 9
backman 3:c5f2281b3ed2 10 #define Debug_cam_uart
backman 11:418e39749f48 11 #define L_eye
backman 6:5a39bde2e016 12 #define R_eye
backman 7:fd976e1ced33 13 #define motor_on
backman 6:5a39bde2e016 14 #define Pcontroller
backman 8:8e49e21d80a2 15 #define task_ma_time
backman 11:418e39749f48 16
backman 14:2d90b0066fc6 17
backman 14:2d90b0066fc6 18 #define car_center 15
backman 14:2d90b0066fc6 19
backman 14:2d90b0066fc6 20
backman 14:2d90b0066fc6 21
backman 14:2d90b0066fc6 22
backman 11:418e39749f48 23 #define t_cam 6
backman 11:418e39749f48 24
backman 11:418e39749f48 25
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 7:fd976e1ced33 40
backman 9:33b99cb45e99 41 // 90/30=3
backman 10:03d5aa2511c4 42 PID cam_to_M_ctrlr(10.0,118.0,0.059,0.105,0.105-0.059,0.00,0.02,10);
backman 7:fd976e1ced33 43
backman 8:8e49e21d80a2 44 DigitalOut task_pin(PTD1);
backman 9:33b99cb45e99 45 TSISensor tsi;
backman 11:418e39749f48 46
backman 11:418e39749f48 47 //os
backman 11:418e39749f48 48 Mutex stdio_mutex;
backman 11:418e39749f48 49
backman 11:418e39749f48 50
backman 14:2d90b0066fc6 51
backman 14:2d90b0066fc6 52
backman 14:2d90b0066fc6 53
backman 14:2d90b0066fc6 54 //static int line3[3]; //special point
backman 11:418e39749f48 55
backman 11:418e39749f48 56 static int b_r_c=0;
backman 14:2d90b0066fc6 57 static int b_l_c=118;
backman 14:2d90b0066fc6 58
backman 11:418e39749f48 59 static int l3_p=0;
backman 11:418e39749f48 60
backman 11:418e39749f48 61 static double v_motor;
backman 11:418e39749f48 62 static double v_servo;
backman 11:418e39749f48 63
backman 11:418e39749f48 64 void cam_thread(void const *args){
backman 9:33b99cb45e99 65
backman 11:418e39749f48 66 while(true){
backman 14:2d90b0066fc6 67
backman 11:418e39749f48 68 cam.read();
backman 11:418e39749f48 69
backman 11:418e39749f48 70
backman 14:2d90b0066fc6 71 b_r_c=cam.black_centerR();
backman 14:2d90b0066fc6 72
backman 14:2d90b0066fc6 73
backman 11:418e39749f48 74
backman 11:418e39749f48 75 Thread::wait(t_cam);
backman 11:418e39749f48 76
backman 11:418e39749f48 77 }
backman 11:418e39749f48 78
backman 11:418e39749f48 79 }
backman 11:418e39749f48 80 // function
backman 11:418e39749f48 81 void de_thread(void const *args){
backman 9:33b99cb45e99 82
backman 11:418e39749f48 83 while(1){
backman 11:418e39749f48 84
backman 11:418e39749f48 85
backman 11:418e39749f48 86
backman 11:418e39749f48 87
backman 11:418e39749f48 88 stdio_mutex.lock();
backman 11:418e39749f48 89 #ifdef Debug_cam_uart
backman 11:418e39749f48 90 #ifdef L_eye
backman 13:63f9a5101205 91 pc.printf("R: ");
backman 13:63f9a5101205 92 for(int i=128;i>=0;i--){
backman 11:418e39749f48 93 if(i==64)
backman 12:fdada4af384a 94 pc.printf("X");
backman 11:418e39749f48 95 else if(i<10)
backman 12:fdada4af384a 96 pc.printf("-");
backman 11:418e39749f48 97 else if(i>117)
backman 13:63f9a5101205 98 pc.printf("-");
backman 11:418e39749f48 99 else
backman 13:63f9a5101205 100 pc.printf("%c", cam.sign_line_imageL[i]);
backman 11:418e39749f48 101 }
backman 13:63f9a5101205 102
backman 12:fdada4af384a 103 pc.printf(" || ");
backman 11:418e39749f48 104 #endif
backman 11:418e39749f48 105 #ifdef R_eye
backman 12:fdada4af384a 106 pc.printf("R: ");
backman 11:418e39749f48 107 for(int i=128;i>=0;i--){
backman 11:418e39749f48 108 if(i==64)
backman 12:fdada4af384a 109 pc.printf("X");
backman 11:418e39749f48 110 else if(i<10)
backman 12:fdada4af384a 111 pc.printf("-");
backman 11:418e39749f48 112 else if(i>117)
backman 12:fdada4af384a 113 pc.printf("-");
backman 11:418e39749f48 114 else
backman 12:fdada4af384a 115 pc.printf("%c", cam.sign_line_imageR[i]);
backman 11:418e39749f48 116 }
backman 12:fdada4af384a 117 pc.printf("\r\n");
backman 11:418e39749f48 118 #endif
backman 14:2d90b0066fc6 119 pc.printf("L: %d mid: %d R: %d center: %d \r\n", );
backman 11:418e39749f48 120 stdio_mutex.unlock();
backman 1:82bc25a7b68b 121
backman 1:82bc25a7b68b 122
backman 11:418e39749f48 123
backman 11:418e39749f48 124
backman 7:fd976e1ced33 125
backman 7:fd976e1ced33 126
backman 11:418e39749f48 127 #endif
backman 11:418e39749f48 128 Thread::wait(1);
backman 7:fd976e1ced33 129
backman 11:418e39749f48 130
backman 11:418e39749f48 131 }
backman 11:418e39749f48 132
backman 11:418e39749f48 133 }
backman 11:418e39749f48 134
backman 11:418e39749f48 135
backman 7:fd976e1ced33 136
backman 7:fd976e1ced33 137
backman 11:418e39749f48 138 void servo_thread(void const *args){
backman 11:418e39749f48 139
backman 11:418e39749f48 140
backman 11:418e39749f48 141 while(1){
backman 11:418e39749f48 142
backman 11:418e39749f48 143
backman 11:418e39749f48 144
backman 14:2d90b0066fc6 145 v_servo=cam_to_M_ctrlr.compute(b_r_c,car_center);
backman 14:2d90b0066fc6 146
backman 11:418e39749f48 147 servo.set_angle(v_servo);
backman 14:2d90b0066fc6 148
backman 11:418e39749f48 149 Thread::wait(20);
backman 11:418e39749f48 150 }
backman 11:418e39749f48 151
backman 11:418e39749f48 152
backman 11:418e39749f48 153
backman 11:418e39749f48 154 }
backman 11:418e39749f48 155
backman 11:418e39749f48 156 void motor_thread(void const *args){
backman 11:418e39749f48 157
backman 11:418e39749f48 158 while(1){
backman 7:fd976e1ced33 159
backman 14:2d90b0066fc6 160 v_motor=pot1.read();
backman 11:418e39749f48 161 MotorA.rotate(v_motor);
backman 11:418e39749f48 162 MotorB.rotate(v_motor);
backman 7:fd976e1ced33 163
backman 14:2d90b0066fc6 164 Thread::wait(10);
backman 11:418e39749f48 165 }
backman 11:418e39749f48 166
backman 11:418e39749f48 167
backman 7:fd976e1ced33 168
backman 11:418e39749f48 169
backman 11:418e39749f48 170 }
backman 11:418e39749f48 171
backman 13:63f9a5101205 172 /*
backman 11:418e39749f48 173 void ctrl_thread(void const *args){
backman 11:418e39749f48 174
backman 11:418e39749f48 175
backman 11:418e39749f48 176
backman 11:418e39749f48 177 while(1){
backman 11:418e39749f48 178
backman 11:418e39749f48 179
backman 11:418e39749f48 180 cam_to_M_ctrlr.compute(b_r_c,64.0);
backman 11:418e39749f48 181
backman 11:418e39749f48 182 Thread::wait(1);
backman 11:418e39749f48 183 }
backman 11:418e39749f48 184
backman 11:418e39749f48 185
backman 11:418e39749f48 186
backman 11:418e39749f48 187
backman 11:418e39749f48 188 }
backman 13:63f9a5101205 189 */
backman 11:418e39749f48 190
backman 11:418e39749f48 191 // global resource
backman 11:418e39749f48 192
backman 11:418e39749f48 193
backman 11:418e39749f48 194
backman 11:418e39749f48 195
backman 11:418e39749f48 196
backman 11:418e39749f48 197
backman 11:418e39749f48 198
backman 11:418e39749f48 199
backman 11:418e39749f48 200 int main() {
backman 11:418e39749f48 201
backman 11:418e39749f48 202 // baud rate init --- no function
backman 12:fdada4af384a 203
backman 12:fdada4af384a 204 pc.baud(115200);
backman 11:418e39749f48 205 /*
backman 11:418e39749f48 206 while(1){
backman 8:8e49e21d80a2 207
backman 11:418e39749f48 208 if(tsi.readPercentage()>0.00011)
backman 11:418e39749f48 209 break;
backman 11:418e39749f48 210 }
backman 11:418e39749f48 211 */
backman 12:fdada4af384a 212 Thread th_c(cam_thread);
backman 11:418e39749f48 213 // Thread thread(ctrl_thread);
backman 14:2d90b0066fc6 214 Thread th_s(servo_thread);
backman 14:2d90b0066fc6 215 Thread th_m(motor_thread);
backman 14:2d90b0066fc6 216 // Thread th_de(de_thread);
backman 11:418e39749f48 217 while(1){
backman 10:03d5aa2511c4 218
backman 11:418e39749f48 219
backman 11:418e39749f48 220 //idle
backman 12:fdada4af384a 221 // stdio_mutex.lock();
backman 12:fdada4af384a 222 // printf("L: %d mid: %d R: %d\r\n",line3[0],line3[1],line3[2]);
backman 11:418e39749f48 223 // stdio_mutex.unlock();
backman 14:2d90b0066fc6 224 // Thread::wait(2000);
backman 12:fdada4af384a 225
backman 7:fd976e1ced33 226 }
backman 7:fd976e1ced33 227
backman 6:5a39bde2e016 228
backman 6:5a39bde2e016 229
backman 7:fd976e1ced33 230
backman 0:68c173249c01 231
backman 0:68c173249c01 232
backman 0:68c173249c01 233
backman 1:82bc25a7b68b 234 return 0;
backman 0:68c173249c01 235
backman 0:68c173249c01 236
backman 0:68c173249c01 237 }