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;
    
    
}