kao yi / Mbed 2 deprecated Boboobooov5

wu

Dependencies:   mbed-rtos mbed

Fork of Boboobooov4 by kao yi

main.cpp@13:63f9a5101205, 2014-06-28 (annotated)

Committer:
backman
Date:
Sat Jun 28 08:37:06 2014 +0000
Revision:
13:63f9a5101205
Parent:
12:fdada4af384a
Child:
14:2d90b0066fc6
rt

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 13:63f9a5101205 28 BX_camera cam(79);
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 13:63f9a5101205 100 pc.printf("R: ");
backman 13:63f9a5101205 101 for(int i=128;i>=0;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 13:63f9a5101205 107 pc.printf("-");
backman 11:418e39749f48 108 else
backman 13:63f9a5101205 109 pc.printf("%c", cam.sign_line_imageL[i]);
backman 11:418e39749f48 110 }
backman 13:63f9a5101205 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 13:63f9a5101205 128 pc.printf("L: %d mid: %d R: %d center : %d \r\n",line3[0],line3[1],line3[2],(line3[0]+line3[2])/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 13:63f9a5101205 156
backman 13:63f9a5101205 157 // pc.printf("angle: %f\r\n",v_servo);
backman 13:63f9a5101205 158 stdio_mutex.lock();
backman 11:418e39749f48 159 servo.set_angle(v_servo);
backman 13:63f9a5101205 160 stdio_mutex.unlock();
backman 11:418e39749f48 161 Thread::wait(20);
backman 11:418e39749f48 162 }
backman 11:418e39749f48 163
backman 11:418e39749f48 164
backman 11:418e39749f48 165
backman 11:418e39749f48 166 }
backman 11:418e39749f48 167
backman 11:418e39749f48 168 void motor_thread(void const *args){
backman 11:418e39749f48 169
backman 11:418e39749f48 170 while(1){
backman 7:fd976e1ced33 171
backman 7:fd976e1ced33 172
backman 11:418e39749f48 173 MotorA.rotate(v_motor);
backman 11:418e39749f48 174 MotorB.rotate(v_motor);
backman 7:fd976e1ced33 175
backman 11:418e39749f48 176 Thread::wait(1);
backman 11:418e39749f48 177 }
backman 11:418e39749f48 178
backman 11:418e39749f48 179
backman 7:fd976e1ced33 180
backman 11:418e39749f48 181
backman 11:418e39749f48 182 }
backman 11:418e39749f48 183
backman 13:63f9a5101205 184 /*
backman 11:418e39749f48 185 void ctrl_thread(void const *args){
backman 11:418e39749f48 186
backman 11:418e39749f48 187
backman 11:418e39749f48 188
backman 11:418e39749f48 189 while(1){
backman 11:418e39749f48 190
backman 11:418e39749f48 191 b_r_c=(double)cam.black_centerR();
backman 11:418e39749f48 192
backman 11:418e39749f48 193 cam_to_M_ctrlr.compute(b_r_c,64.0);
backman 11:418e39749f48 194
backman 11:418e39749f48 195 Thread::wait(1);
backman 11:418e39749f48 196 }
backman 11:418e39749f48 197
backman 11:418e39749f48 198
backman 11:418e39749f48 199
backman 11:418e39749f48 200
backman 11:418e39749f48 201 }
backman 13:63f9a5101205 202 */
backman 11:418e39749f48 203
backman 11:418e39749f48 204 // global resource
backman 11:418e39749f48 205
backman 11:418e39749f48 206
backman 11:418e39749f48 207
backman 11:418e39749f48 208
backman 11:418e39749f48 209
backman 11:418e39749f48 210
backman 11:418e39749f48 211
backman 11:418e39749f48 212
backman 11:418e39749f48 213 int main() {
backman 11:418e39749f48 214
backman 11:418e39749f48 215 // baud rate init --- no function
backman 12:fdada4af384a 216
backman 12:fdada4af384a 217 pc.baud(115200);
backman 11:418e39749f48 218 /*
backman 11:418e39749f48 219 while(1){
backman 8:8e49e21d80a2 220
backman 11:418e39749f48 221 if(tsi.readPercentage()>0.00011)
backman 11:418e39749f48 222 break;
backman 11:418e39749f48 223 }
backman 11:418e39749f48 224 */
backman 12:fdada4af384a 225 Thread th_c(cam_thread);
backman 11:418e39749f48 226 // Thread thread(ctrl_thread);
backman 13:63f9a5101205 227 Thread thread(servo_thread);
backman 11:418e39749f48 228 // Thread thread(motor_thread);
backman 11:418e39749f48 229 Thread th_de(de_thread);
backman 11:418e39749f48 230 while(1){
backman 10:03d5aa2511c4 231
backman 11:418e39749f48 232
backman 11:418e39749f48 233 //idle
backman 12:fdada4af384a 234 // stdio_mutex.lock();
backman 12:fdada4af384a 235 // printf("L: %d mid: %d R: %d\r\n",line3[0],line3[1],line3[2]);
backman 11:418e39749f48 236 // stdio_mutex.unlock();
backman 11:418e39749f48 237 Thread::wait(2000);
backman 12:fdada4af384a 238
backman 7:fd976e1ced33 239 }
backman 7:fd976e1ced33 240
backman 6:5a39bde2e016 241
backman 6:5a39bde2e016 242
backman 7:fd976e1ced33 243
backman 0:68c173249c01 244
backman 0:68c173249c01 245
backman 0:68c173249c01 246
backman 1:82bc25a7b68b 247 return 0;
backman 0:68c173249c01 248
backman 0:68c173249c01 249
backman 0:68c173249c01 250 }