File content as of revision 7:f04bde0ca846:

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

#define Debug_cam_uart

//left   0.015
//middle 0.045
//right  0.072
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() {
  /*  
    int black_va;
    int white_va;
    */
    double left = 0.016, middle = 0.045, right = 0.071;
    double error, turn, last_turn = middle;
    double Kp = 0.000527;
    int black_centerR, black_centerL, center;
    
    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(black_centerL == 128 && black_centerR == 0){//no line
            
            turn = middle;
            
        } else if (black_centerL == 128 && black_centerR != 0){//no left line
        
            turn = (middle + left) / 2;
            
        
        } else if (black_centerL != 128 && black_centerR == 0){//no right line
            
            turn = (middle + right) / 2;
            
        } else {
            if(60 < center && center < 68){
                
                turn = middle;
                
            } else{
                error = 64 - center;
                turn = Kp * error + middle;
            }
        }

        /*if(turn == middle){
            if (last_turn < middle){
                
                servo.set_angle(last_turn);
                wait_ms(500);
                
            } else if (last_turn > middle){
                
                servo.set_angle(last_turn);
                wait_ms(500);
            }
            servo.set_angle(turn);
        } else {
            servo.set_angle(turn);
        }
        last_turn = turn;*/
        
        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\r\n", black_centerL, black_centerR, center, turn);
        
        
        /*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;
    
    
}