kao yi / Mbed 2 deprecated Boboobooov5

wu

Dependencies:   mbed-rtos mbed

Fork of Boboobooov4 by kao yi

main.cpp@17:3dac99cf2b89, 2014-06-30 (annotated)

Committer:
backman
Date:
Mon Jun 30 03:37:05 2014 +0000
Revision:
17:3dac99cf2b89
Parent:
16:b78dce5c0e98
Child:
18:eb675df59c7f
wu

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