kao yi
/
Boboobooov5
wu
Fork of Boboobooov4 by
main.cpp
- Committer:
- backman
- Date:
- 2014-07-02
- Revision:
- 19:4869b10a962e
- Parent:
- 18:eb675df59c7f
- Child:
- 20:30799cbda86b
File content as of revision 19:4869b10a962e:
#include "mbed.h" #include "rtos.h" #include "controller.h" #include "servo_api.h" #include "camera_api.h" #include "motor_api.h" #include "pot.h" #include "TSISensor.h" #include "Stack.h" #define Debug_cam_uart //#define L_eye #define R_eye #define motor_on #define Pcontroller //#define task_ma_time #define R_target 64 #define L_target 64 #define t_cam 6 #define black_center 64 Serial pc(USBTX, USBRX); // tx, rx BX_servo servo; BX_camera cam(0); BX_motor MotorA('A'); BX_motor MotorB('B'); BX_pot pot1('1'); BX_pot pot2('2'); // 90/30=3 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(PTE5); //cam black DigitalOut servo_p(PTE4); //servo coffee DigitalOut idle_p (PTE3); //DigitalOut de_p(PTD3); // red #endif TSISensor tsi; //os Mutex stdio_mutex; //global resource Stack<int> points(10); static int pre_b_r_c; static double v_motor; static double v_servo; void cam_thread(void const *args){ while(true){ #ifdef task_ma_time cam_p=1; #endif cam.read(); #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; #endif cam.read(); stdio_mutex.lock(); #ifdef Debug_cam_uart #ifdef L_eye pc.printf("L: "); for(int i=128;i>=0;i--){ if(i==64) pc.printf("X"); else if(i<10) pc.printf("-"); else if(i>117) pc.printf("-"); else pc.printf("%c", cam.sign_line_imageL[i]); } pc.printf(" || "); #endif #ifdef R_eye pc.printf("R: "); for(int i=128;i>=0;i--){ if(i==64) pc.printf("X"); else if(i<10) pc.printf("-"); else if(i>117) pc.printf("-"); else pc.printf("%c", cam.sign_line_imageR[i]); } #endif 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; #endif #endif Thread::wait(10); } } void servo_thread(void const *args){ while(1){ #ifdef task_ma_time servo_p=1; #endif 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); //if(b_l_c!=-1) // v_servo=cam_to_M_ctrlr.compute(b_l_c,L_target); // v_servo=pot2.read(); #ifdef task_ma_time servo_p=0; #endif Thread::wait(20); } } void motor_thread(void const *args){ while(1){ v_motor=pot1.read(); MotorA.rotate(v_motor); MotorB.rotate(v_motor); Thread::wait(10); } } int main() { // baud rate init --- no function servo.set_angle(0.055); pc.baud(115200); //while(1); /* while(1){ if(tsi.readPercentage()>0.00011) break; } */ Thread th_c(cam_thread); // Thread thread(ctrl_thread); Thread th_s(servo_thread); // Thread th_m(motor_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(1000); } return 0; }