File content as of revision 21:4e8a4f66aaef:

#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 center 64



#define t_cam 4

#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> pointsR(10);
Stack<int> pointsL(10);







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 : R: %d   L: %d \r\n",b_r_c,b_l_c);     
           pointsR.push(b_r_c);           
           pointsL.push(b_l_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=127;i>=0;i--){
             if(i==64||i==0||i==127)
               pc.printf("X");
             else          
               pc.printf("%c", cam.sign_line_imageL[i]);         
         }
         
         pc.printf(" || ");
      #endif
      #ifdef R_eye
        pc.printf("R: ");
        for(int i=127;i>=0;i--){
             if(i==64||i==0||i==127)
               pc.printf("X");
             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
        
      //  servo.set_angle(0.055);
       int point;
       int sum_error_R=0;
       int sum_error_L=0;
       int n_pointR=0;
       int n_pointL=0;
       n_pointR=pointsR.available() ;
       int correct_pointR_number=0;
       n_pointL=pointsL.available() ;
       int correct_pointL_number=0;
       
       pc.printf("R: ");
       
        for(int i=0;i<n_pointR;i++){
             pointsR.pop(&point);
             pc.printf("%d ",point);
        //algorithm
            if(point>0){
                sum_error_R+=point;   //because R's black is on the right side of line
                correct_pointR_number++;
            }
        }
        
pc.printf("L:");
        for(int i=0;i<n_pointL;i++){
                pointsL.pop(&point);
                pc.printf("%d ",point);
            if(point>0){
                sum_error_L+=point;   //because R's black is on the right side of line
                correct_pointL_number++;
             }
       }
        pc.printf("\r\n");
        int error_R_ave=(correct_pointR_number==0)?0:sum_error_R/correct_pointR_number;
        int error_L_ave=(correct_pointL_number==0)?0:sum_error_L/correct_pointL_number;
        
      //  pc.printf("L: %d  R: %d\r\n",error_L_ave,error_R_ave);
        
        
        //two line
        if(error_L_ave!=0 && error_R_ave!=0){
            servo.set_angle(cam_to_M_ctrlr.compute((error_L_ave+error_R_ave)/2,center) );
        }
        
        //in the correct think, one line should not appear
        //right line
        else if(error_L_ave==0 && error_R_ave!=0){  
            servo.set_angle(cam_to_M_ctrlr.compute(error_R_ave,30));
        }
        //left line
        else if(error_R_ave==0 && error_L_ave!=0){
            servo.set_angle(cam_to_M_ctrlr.compute(error_L_ave,88));
        }
        //no line
        else if(error_L_ave!=0 && error_R_ave!=0){}
        
        
       // cam_to_M_ctrlr.compute(black_center+sum_error_R/n_pointR,center);  
     
       // servo.set_angle(         cam_to_M_ctrlr.compute(black_center+sum_e/n_pointR,center) );
         
        // 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;
    
    
}