kao yi / Mbed 2 deprecated Boboobooov5

wu

Dependencies:   mbed-rtos mbed

Fork of Boboobooov4 by kao yi

main.cpp@15:585df3979be8, 2014-06-29 (annotated)

Committer:
backman
Date:
Sun Jun 29 14:02:25 2014 +0000
Revision:
15:585df3979be8
Parent:
14:2d90b0066fc6
Child:
16:b78dce5c0e98
lin

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