File content as of revision 9:c318ee86d6e9:

#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() {
  /*  
    int black_va;
    int white_va;
    */
    double left = 0.025, middle = 0.041, right = 0.057;
    //double left = 0.04, middle = 0.058, right = 0.076;
    double error, turn, last_turn =0.0;
    double Kp = 0.00025;
    double v = 0.3;
    int black_centerR, black_centerL, center;
    
    char psudo_line[128];
    int times = 0;
    double avg = middle;
    
#ifdef Debug_cam_uart
     pc.baud(115200);
     
     
  while(1){   
     
        cam.read();
        
        
        //black_centerL = cam.black_centerL();
        center = cam.black_centerL();
        black_centerR = cam.black_centerR();
        /*int R = 64 - black_centerR;
        int L = 64 - black_centerL;
        if( L > 0 && R > 0){
            center = (L >= R )? black_centerL : black_centerR;
        }
        else if(black_centerL < 0 && black_centerR > 0){
            center = ((-1)*L >= R )? black_centerL : black_centerR;
        }
        else if(black_centerL > 0 && black_centerR < 0){
            center = (L >= (-1)*R )? black_centerL : black_centerR;
        }
        else{
            center = ((-1)*L >= (-1)*R )? black_centerL : black_centerR;
        }*/
        
        //center = (black_centerL + black_centerR) / 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(center<68 &&center>60) 
            turn = middle;
        else 
            turn = middle + Kp *(64-center);
        
        
        servo.set_angle(turn);
        //MotorA.rotate(0.3);
        //MotorB.rotate(0.3);
        //last_turn += turn;
        //times++;
        //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  center:  %d    turn:   %lf\r\n", black_centerL, black_centerR,center,turn);
       
       }
       

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


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

       
         
         
     
     
  #endif   
       
     
     
     
     
     
     
     
     
     
     
     
    
    
    
    
    return 0;
    
    
}