main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "controller.h"
00004 #include "servo_api.h"
00005 #include "camera_api.h"
00006 #include "motor_api.h"
00007 #include "pot.h"
00008 #include "TSISensor.h"
00009 
00010 #define Debug_cam_uart
00011 #define L_eye
00012 #define R_eye
00013 #define motor_on 
00014 #define Pcontroller
00015 #define task_ma_time
00016 
00017 #define t_cam 6
00018 
00019 
00020 
00021 
00022 
00023 
00024 
00025 
00026 
00027 BX_servo servo; 
00028 
00029 BX_camera cam;
00030 
00031 BX_motor MotorA('A');
00032 BX_motor MotorB('B');
00033 
00034 BX_pot pot1('1');
00035 
00036                                  // 90/30=3
00037 PID cam_to_M_ctrlr(10.0,118.0,0.059,0.105,0.105-0.059,0.00,0.02,10);
00038 
00039 DigitalOut task_pin(PTD1);
00040 TSISensor tsi;
00041 
00042 //os
00043 Mutex stdio_mutex; 
00044 
00045 
00046 /*
00047 void led2_thread(void const *args) {
00048    while (true) {
00049         led2 = !led2;
00050         Thread::wait(1000);
00051     }
00052 }
00053 */
00054 
00055 static int b_r_c=0;
00056 static int b_l_c=0;
00057 static int line3[3]; //special point 
00058 static int l3_p=0;
00059 
00060 static double v_motor;
00061 static double v_servo;
00062 
00063 void cam_thread(void const *args){
00064     
00065     while(true){
00066         cam.read();
00067         
00068         b_l_c=(double)cam.black_centerL();
00069         
00070         if(l3_p==0){
00071         
00072             line3[l3_p]=(double)cam.black_centerR();
00073         }else if(l3_p==1){
00074             line3[l3_p]=0;
00075             
00076         }else {
00077            line3[l3_p]=(double)cam.black_centerL();    
00078                
00079         }        
00080                
00081         l3_p++;
00082         if(l3_p==3)
00083             l3_p=0;
00084             
00085         Thread::wait(t_cam);
00086         
00087         }
00088     
00089 }
00090 // function
00091 void de_thread(void const *args){
00092     
00093     while(1){
00094         
00095         
00096           cam.read();
00097         
00098         b_l_c=(double)cam.black_centerL();
00099         
00100         if(l3_p==0){
00101         
00102             line3[l3_p]=(double)cam.black_centerR();
00103         }else if(l3_p==1){
00104             line3[l3_p]=0;
00105             
00106         }else {
00107            line3[l3_p]=(double)cam.black_centerL();    
00108                
00109         }        
00110                
00111         l3_p++;
00112         if(l3_p==3)
00113             l3_p=0;
00114         
00115         
00116     stdio_mutex.lock();    
00117     #ifdef Debug_cam_uart 
00118       #ifdef L_eye  
00119         printf("L: ");
00120         for(int i=0;i<128;i++){
00121              if(i==64)
00122                printf("X");
00123              else if(i<10)
00124                printf("-");
00125              else if(i>117)
00126               printf("-");  
00127                
00128              else          
00129                printf("%c", cam.sign_line_imageL[i]);
00130          }
00131          printf("           ||             ");
00132       #endif
00133       #ifdef R_eye
00134         printf("R: ");
00135         for(int i=128;i>=0;i--){
00136              if(i==64)
00137                printf("X");
00138              else if(i<10)
00139                printf("-");
00140              else if(i>117)
00141                printf("-");
00142              else          
00143                printf("%c", cam.sign_line_imageR[i]);         
00144          }
00145          printf("\r\n");
00146      #endif
00147          printf("L: %d  mid: %d R: %d\r\n",line3[0],line3[1],line3[2]);
00148          stdio_mutex.unlock();
00149      
00150          
00151      
00152        
00153          
00154          
00155 #endif   
00156              Thread::wait(1);
00157 
00158       
00159     }
00160 
00161 }
00162 
00163 
00164 
00165 
00166 void servo_thread(void const *args){
00167     
00168 
00169         while(1){
00170 
00171              int ctrl=(line3[0]+line3[2])/2;
00172             
00173         
00174              v_servo=cam_to_M_ctrlr.compute(ctrl,118.0);
00175              servo.set_angle(v_servo);
00176             
00177          Thread::wait(20);    
00178         }    
00179     
00180     
00181     
00182 }
00183 
00184 void motor_thread(void const *args){
00185     
00186         while(1){
00187             
00188             
00189              MotorA.rotate(v_motor);
00190              MotorB.rotate(v_motor);
00191             
00192             Thread::wait(1);    
00193         }
00194       
00195       
00196       
00197     
00198     }
00199 
00200 
00201 void ctrl_thread(void const *args){
00202     
00203     
00204     
00205       while(1){
00206           
00207          b_r_c=(double)cam.black_centerR();
00208 
00209          cam_to_M_ctrlr.compute(b_r_c,64.0);
00210           
00211           Thread::wait(1); 
00212       }    
00213     
00214     
00215     
00216     
00217 }
00218 
00219 
00220 // global resource
00221 
00222 
00223 
00224 
00225 
00226 
00227 
00228 
00229 int main() {
00230     
00231 // baud rate init --- no function
00232      
00233     
00234    /*
00235       while(1){
00236          
00237              if(tsi.readPercentage()>0.00011)
00238              break;
00239       }
00240     */
00241     //  Thread th_c(cam_thread);
00242    //   Thread thread(ctrl_thread);  
00243    //   Thread thread(servo_thread);  
00244    //   Thread thread(motor_thread);     
00245         Thread th_de(de_thread);  
00246      while(1){
00247          
00248            
00249      //idle
00250      //stdio_mutex.lock();
00251      //   printf("L: %d  mid: %d R: %d\r\n",line3[0],line3[1],line3[2]);
00252     // stdio_mutex.unlock();     
00253           Thread::wait(2000);
00254      
00255      }
00256    
00257      
00258      
00259     
00260     
00261     
00262     
00263     return 0;
00264     
00265     
00266 }