File content as of revision 10:e11f82b40d53:

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

#define Debug_cam_uart


Serial pc(USBTX, USBRX);
BX_servo servo; 
BX_camera cam;
BX_motor MotorA('A');
BX_motor MotorB('B');

//PID cam_to_M_ctrlr(10.0, 118.0, 0.06, 0.11, (0.104/30), 0.0, 0.0, 10);


int main() {
  
    double left = 0.025, middle = 0.038, right = 0.054;
    double error, turn, last_turn = middle, avg = 0;
    double Kp = 0.000527, Ki = 0.025;
    int black_centerR, black_centerL, center, times = 0;
    
    char psudo_line[128];
    
#ifdef Debug_cam_uart
     pc.baud(115200);
     
     
  while(1){   
     
        cam.read();

        MotorA.rotate(0.0);
        MotorB.rotate(0.0);
        
        black_centerL = cam.black_centerL();
        black_centerR = cam.black_centerR();
        center = (black_centerL + black_centerR) / 2;
        
        MotorA.rotate(0.2);
        MotorB.rotate(0.2);
        

        if(times % 10 == 0){
            avg = last_turn / 10;
            last_turn = 0;



            if(times == 30){
                times = 0;
                if(avg < middle-0.003){

                    avg -= 0.002;

                }
                else if(avg > middle + 0.003){

                    avg += 0.002;
                }
            }
        }

        if(black_centerL == 128 && black_centerR == 0){//no line
            
            turn = avg;
            
        } else if (black_centerL == 128 && black_centerR != 0){//no left line
            

            /*error = 64 - black_centerR;
            turn = Kp * error + middle;*/
            turn = avg;

            
        
        } else if (black_centerL != 128 && black_centerR == 0){//no right line
            

            /*error = 64 - black_centerL;
            turn = Kp * error + middle;*/
            turn = avg;
            
        } else {

            if(60 < center && center < 68){
                
                turn = middle;
                
            } else{
                error = 64 - center;
                turn = Kp * error + middle;
            }
        }

        last_turn += turn;
        times++;
        servo.set_angle(turn);
        
        
        //output the psudo map
        for(int i = 127; i >= 0; i--)   
            psudo_line[i] = '0';
        
        for(int i = center-5; i < center+5; i++)
            psudo_line[i] = ' ';
        
        for(int i = 117; i > 10; i--){
            pc.printf("%c", psudo_line[i]);    
        }
        pc.printf("\r\n");
        pc.printf("black centerL: %d   black_centerR: %d   psudo_line: %d turn: %lf avg: %lf\r\n", black_centerL, black_centerR, center, turn, avg);
        
        
        /*for(int i = 128; i > 64;i--){
             if(i==64)
               pc.printf("X");
             else          
               pc.printf("%c", cam.sign_line_imageL[i]);
         }
         pc.printf("           ||             ");
         
         for(int i = 64; i > 0; i--){
             if(i==64)
               pc.printf("X");
             else          
               pc.printf("%c", cam.sign_line_imageR[i]);
         }
         pc.printf("\r\n");
         pc.printf("black_centerL: %d   black_centerR: %d\r\n", black_centerL, black_centerR);
       */
       }

        
     
     
     
     
     
     
        //   pc.printf("ang :%d\r\n ",( (64.0-center) /64.0  )*90);
   //--------------------------------------------            


         // servo.set_angle(( (64.0-center) /64.0  )*90 );

       
         
         
     
     
  #endif   
       
     
     
     
     
     
     
     
     
     
     
     
    
    
    
    
    return 0;
    
    
}