kao yi / Mbed 2 deprecated Bov3

wu

Dependencies:   mbed-rtos mbed

Fork of CCC by kao yi

main.cpp@11:418e39749f48, 2014-06-28 (annotated)

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