File content as of revision 11:ffd762ae141b:

#include "mbed.h"
#include "controller.h"
#include "servo_api.h"
#include "camera_api.h"
#include "motor_api.h"
#include "pot.h"


#define Debug_cam_uart
#define R_eye
#define motor_on 
#define Pcontroller
#define task_ma_time
#include "TSISensor.h"

Serial pc(USBTX, USBRX);


BX_servo servo; 

BX_camera cam;

BX_motor MotorA('A');
BX_motor MotorB('B');

BX_pot pot1('1');

                                 // 90/30=3
PID cam_to_M_ctrlr(10.0,118.0,0.06,0.11,(0.104/30),0.0,0.0,10);
//PID(float in_min,float in_max,float out_min,float out_max,float Kc, float tauI, float tauD, float interval)
//in_min in_out  camera array
//out_min out_max servo range
//

DigitalOut task_pin(PTD1);
TSISensor tsi;
int main() {
    
    
       pc.baud(115200);
     while(1){
         
       if(tsi.readPercentage()>0.00011)
           break;
     }
     
    
     
     
      double motor;
     double b_r_c;
     double PID_v;
     
     double Kp = 0.0005, Ki = 0.0001, Kd = 0.0001;
     double integral = 0, derivative = 0, error, last_error = 0, turn;
     
     while(1){
         
        #ifdef task_ma_time
             task_pin=1;
        #endif 
         
         
         
        cam.read();  
     
#ifdef Debug_cam_uart
      
      #ifdef L_eye  
        for(int i=0;i<128;i++){

              
              
              
             if(i==64)
               pc.printf("X");
             else          
               pc.printf("%c", cam.sign_line_imageL[i]);
         }
         pc.printf("           ||             ");
      #endif
      #ifdef R_eye
        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]);
         
           
            
            
               
         
         }
         pc.printf("\r\n");
         
         
       
     
     #endif
         
         
     
            
     
     
  
     
   

#endif   
     
      
     
     
     
     
     
     
     
       
         #ifdef motor_on 
           
     
             motor=pot1.read();
     
    
    
  
             MotorA.rotate(motor);
             MotorB.rotate(motor);
        #endif
       
       
       
       
       
               b_r_c=(double)cam.black_centerR();

         //PID_v=cam_to_M_ctrlr.compute(b_r_c,15.0);
         //pc.printf("%f %d %d speed :%f  bk_center %f PID:%f \r\n",cam_to_M_ctrlr.de_v,cam.de_v,cam.de_v2,motor,b_r_c,PID_v);
            pc.printf("b_r_c       %lf\r\n",b_r_c);
            
            error = b_r_c - 64;
            integral = 0.3 * integral + error;  //0.3 is the factor to make old datas become smaller
            derivative = error - last_error;
            
            turn = error*Kp + integral*Ki + derivative*Kd;
            
            if(error < 8 && error > -8){
                servo.set_angle(0.085);
            }
            else{
                servo.set_angle(0.085 + turn);
            }
            
            leas_error = error;
         
        //servo.set_angle(PID_v);
        
        
       
     
      #ifdef task_ma_time
        task_pin=0;
        #endif 
        
     }
   
     
     
    
    
    
    
    return 0;
    
    
}