kao yi
/
Boboobooov5
wu
Fork of Boboobooov4 by
Diff: main.cpp
- Revision:
- 19:4869b10a962e
- Parent:
- 18:eb675df59c7f
- Child:
- 20:30799cbda86b
--- a/main.cpp Mon Jun 30 07:01:58 2014 +0000 +++ b/main.cpp Wed Jul 02 03:23:07 2014 +0000 @@ -9,11 +9,11 @@ #include "Stack.h" #define Debug_cam_uart -#define L_eye -//#define R_eye +//#define L_eye +#define R_eye #define motor_on #define Pcontroller -#define task_ma_time +//#define task_ma_time #define R_target 64 @@ -21,9 +21,9 @@ -#define t_cam 2 +#define t_cam 6 - +#define black_center 64 Serial pc(USBTX, USBRX); // tx, rx @@ -44,9 +44,10 @@ PID cam_to_M_ctrlr(10.0,118.0,0.046,0.083,0.083-0.046,0.00,0.00,10); #ifdef task_ma_time -DigitalOut cam_p(PTD1); //cam black -DigitalOut servo_p(PTA13); //servo coffee -DigitalOut de_p(PTD3); // red +DigitalOut cam_p(PTE5); //cam black +DigitalOut servo_p(PTE4); //servo coffee +DigitalOut idle_p (PTE3); +//DigitalOut de_p(PTD3); // red #endif TSISensor tsi; @@ -64,8 +65,7 @@ -static int b_r_c=64; -static int b_l_c=64; + static int pre_b_r_c; static double v_motor; @@ -80,37 +80,52 @@ while(true){ #ifdef task_ma_time cam_p=1; - #endif + #endif + cam.read(); - - b_r_c=cam.black_centerR(); - // b_l_c=cam.black_centerL(); - - points.push(b_r_c); - - + #ifdef task_ma_time cam_p=0; #endif + + int b_r_c,b_l_c; + + + b_r_c=cam.black_centerR(); + // b_l_c=cam.black_centerL(); + + //right + // printf("push :%d\r\n",b_r_c); + points.push(b_r_c); + + Thread::wait(t_cam); - + } } // function + + + + + + + + void de_thread(void const *args){ while(1){ #ifdef task_ma_time - de_p=0; + //de_p=0; #endif - + cam.read(); stdio_mutex.lock(); #ifdef Debug_cam_uart #ifdef L_eye @@ -145,18 +160,20 @@ pc.printf("\r\n Rcenter :%d Lcenter : %d servo: %f \r\n",cam.black_centerR(),cam.black_centerL(),v_servo); + pc.printf("stack n: %d",points.available()); + stdio_mutex.unlock(); #ifdef task_ma_time - de_p=1; + //de_p=1; #endif #endif - Thread::wait(1); + Thread::wait(10); } @@ -175,30 +192,44 @@ servo_p=1; #endif - int point; - - for(;points.available()!=0;){ - points.pop(&point); - + int point; + int sum_e=0; + int n_point=0; + n_point=points.available() ; + + for(int i=0;i<n_point;i++){ + points.pop(&point); //algorithm - + + sum_e+=point-black_center; + } - + printf("error :%d compute: %f\r\n",sum_e/n_point, cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target)); + + + cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target); + // pc.printf("expect :%d \r\n",black_center+sum_e/n_point ); + // pc.printf("angle :%f", cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target) ); + servo.set_angle( cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target) ); + + // pc.printf("dfdf"); //if(b_r_c!=-1) - v_servo=cam_to_M_ctrlr.compute(b_r_c,R_target); + // v_servo=cam_to_M_ctrlr.compute(b_r_c,R_target); //if(b_l_c!=-1) // v_servo=cam_to_M_ctrlr.compute(b_l_c,L_target); // v_servo=pot2.read(); - servo.set_angle(v_servo); + #ifdef task_ma_time servo_p=0; #endif - Thread::wait(20); + Thread::wait(20); + + } @@ -241,14 +272,21 @@ servo.set_angle(0.055); pc.baud(115200); - /* - while(1){ + + + + //while(1); + + + + + /* while(1){ if(tsi.readPercentage()>0.00011) break; } - */ - + + */ @@ -256,15 +294,27 @@ // Thread thread(ctrl_thread); Thread th_s(servo_thread); // Thread th_m(motor_thread); - // Thread th_de(de_thread); + // Thread th_de(de_thread); + + //Thread dddd(pin2_thread); while(1){ + #ifdef task_ma_time + idle_p=1; + #endif + + + + #ifdef task_ma_time + idle_p=0; + #endif + //idle // stdio_mutex.lock(); // printf("L: %d mid: %d R: %d\r\n",line3[0],line3[1],line3[2]); // stdio_mutex.unlock(); - // Thread::wait(2000); + Thread::wait(1000); }