Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of Boboobooov4 by
main.cpp
- Committer:
- backman
- Date:
- 2014-07-02
- Revision:
- 20:30799cbda86b
- Parent:
- 19:4869b10a962e
File content as of revision 20:30799cbda86b:
#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); 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; int compute_p=0; printf("stak size: %d\r\n",n_point); for(int i=0; points.pop(&point)==0;i++){ printf("pop :%d\r\n",point); //algorithm if(point!=-1){ sum_e+=point; n_point++; } } printf("error :%d compute: %f\r\n",(sum_e/(n_point/2)), cam_to_M_ctrlr.compute(R_target+sum_e/n_point,R_target)); // v_servo=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) ); //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(); servo.set_angle(v_servo); #ifdef task_ma_time servo_p=0; #endif Thread::wait(1000); } } 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 // pc.printf("idle\r\n"); for(int i=0;i<6;i++){ // printf("push :%d\r\n",10+i*10); points.push(0+i*10); } printf("idle\r\n"); Thread::wait(1000); #ifdef task_ma_time idle_p=0; #endif } return 0; }