kao yi / Mbed 2 deprecated Boboobooov5

wu

Dependencies:   mbed-rtos mbed

Fork of Boboobooov4 by kao yi

main.cpp@12:fdada4af384a, 2014-06-28 (annotated)

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