kao yi / Mbed 2 deprecated Boboobooov5

wu

Dependencies:   mbed-rtos mbed

Fork of Boboobooov4 by kao yi

main.cpp@16:b78dce5c0e98, 2014-06-29 (annotated)

Committer:
backman
Date:
Sun Jun 29 14:32:30 2014 +0000
Revision:
16:b78dce5c0e98
Parent:
15:585df3979be8
Child:
17:3dac99cf2b89
two cam;

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 15:585df3979be8 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 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 13:63f9a5101205 96 pc.printf("R: ");
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 15:585df3979be8 122 pc.printf("\r\n center :%d servo: %f \r\n",cam.black_centerR(),v_servo);
backman 11:418e39749f48 123 #endif
backman 15:585df3979be8 124
backman 11:418e39749f48 125 stdio_mutex.unlock();
backman 1:82bc25a7b68b 126
backman 1:82bc25a7b68b 127
backman 11:418e39749f48 128
backman 11:418e39749f48 129
backman 7:fd976e1ced33 130
backman 7:fd976e1ced33 131
backman 11:418e39749f48 132 #endif
backman 11:418e39749f48 133 Thread::wait(1);
backman 7:fd976e1ced33 134
backman 11:418e39749f48 135
backman 11:418e39749f48 136 }
backman 11:418e39749f48 137
backman 11:418e39749f48 138 }
backman 11:418e39749f48 139
backman 11:418e39749f48 140
backman 7:fd976e1ced33 141
backman 7:fd976e1ced33 142
backman 11:418e39749f48 143 void servo_thread(void const *args){
backman 11:418e39749f48 144
backman 11:418e39749f48 145
backman 11:418e39749f48 146 while(1){
backman 15:585df3979be8 147
backman 16:b78dce5c0e98 148 if(b_r_c!=-1)
backman 16:b78dce5c0e98 149 v_servo=cam_to_M_ctrlr.compute(b_r_c,R_target);
backman 16:b78dce5c0e98 150 if(b_l_c!=-1)
backman 16:b78dce5c0e98 151 v_servo=cam_to_M_ctrlr.compute(b_l_c,L_target);
backman 16:b78dce5c0e98 152
backman 15:585df3979be8 153
backman 15:585df3979be8 154
backman 11:418e39749f48 155
backman 15:585df3979be8 156
backman 15:585df3979be8 157 // v_servo=pot2.read();
backman 15:585df3979be8 158 servo.set_angle(v_servo);
backman 15:585df3979be8 159
backman 11:418e39749f48 160 Thread::wait(20);
backman 11:418e39749f48 161 }
backman 11:418e39749f48 162
backman 11:418e39749f48 163
backman 11:418e39749f48 164
backman 11:418e39749f48 165 }
backman 11:418e39749f48 166
backman 15:585df3979be8 167
backman 15:585df3979be8 168
backman 15:585df3979be8 169
backman 15:585df3979be8 170
backman 15:585df3979be8 171
backman 15:585df3979be8 172
backman 11:418e39749f48 173 void motor_thread(void const *args){
backman 11:418e39749f48 174
backman 11:418e39749f48 175 while(1){
backman 7:fd976e1ced33 176
backman 14:2d90b0066fc6 177 v_motor=pot1.read();
backman 11:418e39749f48 178 MotorA.rotate(v_motor);
backman 11:418e39749f48 179 MotorB.rotate(v_motor);
backman 7:fd976e1ced33 180
backman 14:2d90b0066fc6 181 Thread::wait(10);
backman 11:418e39749f48 182 }
backman 11:418e39749f48 183
backman 11:418e39749f48 184
backman 7:fd976e1ced33 185
backman 11:418e39749f48 186
backman 11:418e39749f48 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 int main() {
backman 11:418e39749f48 198
backman 11:418e39749f48 199 // baud rate init --- no function
backman 12:fdada4af384a 200
backman 15:585df3979be8 201 servo.set_angle(0.055);
backman 12:fdada4af384a 202 pc.baud(115200);
backman 11:418e39749f48 203 /*
backman 11:418e39749f48 204 while(1){
backman 8:8e49e21d80a2 205
backman 11:418e39749f48 206 if(tsi.readPercentage()>0.00011)
backman 11:418e39749f48 207 break;
backman 11:418e39749f48 208 }
backman 11:418e39749f48 209 */
backman 15:585df3979be8 210
backman 15:585df3979be8 211
backman 15:585df3979be8 212
backman 15:585df3979be8 213
backman 12:fdada4af384a 214 Thread th_c(cam_thread);
backman 11:418e39749f48 215 // Thread thread(ctrl_thread);
backman 14:2d90b0066fc6 216 Thread th_s(servo_thread);
backman 14:2d90b0066fc6 217 Thread th_m(motor_thread);
backman 16:b78dce5c0e98 218 // Thread th_de(de_thread);
backman 11:418e39749f48 219 while(1){
backman 10:03d5aa2511c4 220
backman 11:418e39749f48 221
backman 11:418e39749f48 222 //idle
backman 12:fdada4af384a 223 // stdio_mutex.lock();
backman 12:fdada4af384a 224 // printf("L: %d mid: %d R: %d\r\n",line3[0],line3[1],line3[2]);
backman 11:418e39749f48 225 // stdio_mutex.unlock();
backman 14:2d90b0066fc6 226 // Thread::wait(2000);
backman 12:fdada4af384a 227
backman 7:fd976e1ced33 228 }
backman 7:fd976e1ced33 229
backman 6:5a39bde2e016 230
backman 6:5a39bde2e016 231
backman 7:fd976e1ced33 232
backman 0:68c173249c01 233
backman 0:68c173249c01 234
backman 0:68c173249c01 235
backman 1:82bc25a7b68b 236 return 0;
backman 0:68c173249c01 237
backman 0:68c173249c01 238
backman 0:68c173249c01 239 }