File content as of revision 7:fe8665daf3e7:

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

#define Debug_cam_uart
#define buffer_size  3
Serial pc(USBTX, USBRX);
BX_servo servo; 
BX_camera cam;
BX_motor motorA('A');
BX_motor motorB('B');


int main() {
    
  /*  
    int black_va;
    int white_va;
    */
    int left_point_buffer[buffer_size] = {0,0,0};
    int right_point_buffer[buffer_size] = {0,0,0};
    int left_length_buffer[buffer_size] = {0,0,0};
    int right_length_buffer[buffer_size] = {0,0,0};
    int error_buffer[buffer_size]= {0,0,0};
    bool isLeftOut = false;
    bool isRightOut = false;
    servo.set_angle(0);

#ifdef Debug_cam_uart
     pc.baud(115200);
     
     motorA.rotate(0.5);
     motorB.rotate(0.5);
     
  while(1){   
     
    cam.read();
      
    //====== find left line's right edging ======
    bool left_isFistPoint = true;
    int left_temp_length = 0;
    int left_max_length = 0;
    int left_temp_point = 0;
    int left_target_point = 0;  
    int left_white_count = 0;
    int left_white_threshold = 2;
    // scan camera data from right to left
    for(int i=117;i>=10;i--){
       // when scan result is black and it is a start point
       if(cam.sign_line_imageL[i] == ' ' && left_isFistPoint){
            left_temp_point = i;
            left_isFistPoint = false;
            if(i == 10 && (left_max_length < left_temp_length)){
                 left_max_length = left_temp_length;
                 left_target_point = left_temp_point;
            }
       }
       // when scan result is black and it is not a start point
       else if(cam.sign_line_imageL[i] == ' ' && !left_isFistPoint){
            left_temp_length++;
            if(i == 10 && (left_max_length < left_temp_length)){
                 left_max_length = left_temp_length;
                 left_target_point = left_temp_point;
            }
       }
       // when scan result is white and there is no black behind it
       else if(cam.sign_line_imageL[i] != ' ' && left_isFistPoint){
           //do nothng
       }
       //when scan result is white and there are some black behind it
       else if(cam.sign_line_imageL[i] != ' '  && !left_isFistPoint){
            left_white_count++;
            //this white is still in the threshold
            if(left_white_count <= left_white_threshold){
               left_temp_length++;
               if(i == 10 && (left_max_length < left_temp_length)){
                 left_max_length = left_temp_length;
                 left_target_point = left_temp_point;
               }
            }
            //this white is out of threshold
            else{
                left_isFistPoint = true;
                if(left_max_length < left_temp_length){
                   left_max_length = left_temp_length;
                   left_target_point = left_temp_point;
                }
                left_temp_length = 0;
                left_white_count = 0;
            }
       }
    }
    
    //average data
    int left_average_point = 0;
    int left_average_length = 0;
    
    //check line
    bool isLeftAllWhite = true;
    int left_black_count = 0;
    for(int i=10;i<=117;i++){
        if(cam.sign_line_imageL[i] == ' ')
        {
            left_black_count++;
            if( left_black_count >= 3 )
            {
                isLeftAllWhite = false;
            }
        }
    }
    if(isLeftAllWhite){
       //do not update the data
       //left_average_point =  (left_point_buffer[0] + left_point_buffer[1] + left_point_buffer[2])/buffer_size;
       //left_average_length = (left_length_buffer[0] + left_length_buffer[1] + left_length_buffer[2])/buffer_size;
        left_average_point = left_point_buffer[buffer_size-1];
        left_average_length = left_length_buffer[buffer_size-1];
        
    }
    else{
     //average the history data
     //left_average_point =  (left_point_buffer[0] + left_point_buffer[1] + left_point_buffer[2] + left_target_point)/(buffer_size+1);
     //left_average_length = (left_length_buffer[0] + left_length_buffer[1] + left_length_buffer[2]  + left_max_length)/(buffer_size+1);
             
     left_average_point = left_target_point;
     left_average_length = left_max_length;
     
     //update the buffer
     for(int i=0;i<buffer_size - 1;i++){
         left_point_buffer[i] = left_point_buffer[i+1];
         left_length_buffer[i] = left_length_buffer[i+1];
     }
     left_point_buffer[buffer_size-1] = left_average_point;
     left_length_buffer[buffer_size-1] = left_average_length;
    }
    
    
    //====== find right line's left edging ======
    bool right_isFistPoint = true;
    int right_temp_length = 0;
    int right_max_length = 0;
    int right_temp_point = 0;
    int right_target_point = 0;  
    int right_white_count = 0;
    int right_white_threshold = 2;
    //scan camera data from right to left
    for(int i=10;i<=117;i++){
       //when scan result is black and it is a start point
       if(cam.sign_line_imageR[i] == ' ' && right_isFistPoint){
            right_temp_point = i;
            right_isFistPoint = false;
            if( i == 117 && (right_max_length < right_temp_length)){
                 right_max_length = right_temp_length;
                 right_target_point = right_temp_point;
            }
       }
       //when scan result is black and it is not a start point
       else if(cam.sign_line_imageR[i] == ' ' && !right_isFistPoint){
            right_temp_length++;
            if( i == 117 && (right_max_length < right_temp_length)){
                 right_max_length = right_temp_length;
                 right_target_point = right_temp_point;
            }
       }
       //when scan result is white and there is no black behind it
       else if(cam.sign_line_imageR[i] != ' ' && right_isFistPoint){
           //do nothng
       }
       //when scan result is white and there are some black behind it
       else if(cam.sign_line_imageR[i] != ' ' && !right_isFistPoint){
            right_white_count++;
            //this white is still in the threshold
            if(right_white_count <= right_white_threshold){
               right_temp_length++;
               if( i == 117 && (right_max_length < right_temp_length)){
                 right_max_length = right_temp_length;
                 right_target_point = right_temp_point;
               }
            }
            //this white is out of threshold
            else{
                right_isFistPoint = true;
                if(right_max_length < right_temp_length){
                   right_max_length = right_temp_length;
                   right_target_point = right_temp_point;
                }
                right_temp_length = 0;
                right_white_count = 0;
            }
       }
    }
        
    //average  data
    int right_average_point =  0;
    int right_average_length = 0;
    //check line
    bool isRightAllWhite = true;
    int right_black_count = 0;
    for(int i=10;i<=117;i++){
        if(cam.sign_line_imageR[i] == ' ')
        {
            right_black_count++;
            if( right_black_count >= 3 )
            {
                isRightAllWhite = false;
            }
        }
    }

    if(isRightAllWhite){
        //do not update the buffer
        //right_average_point =  (right_point_buffer[0] + right_point_buffer[1] + right_point_buffer[2])/buffer_size;
        //right_average_length = (right_length_buffer[0] + right_length_buffer[1] + right_length_buffer[2])/buffer_size; 
        right_average_point = right_point_buffer[buffer_size-1];
        right_average_length = right_length_buffer[buffer_size-1];
    }
    else{
        //average the history data
        //right_average_point =  (right_point_buffer[0] + right_point_buffer[1] + right_point_buffer[2] + right_target_point)/(buffer_size+1);
        //right_average_length = (right_length_buffer[0] + right_length_buffer[1] + right_length_buffer[2] + right_max_length)/(buffer_size+1); 
        right_average_point = right_target_point;
        right_average_length = right_max_length;
        
        //update the buffer
        for(int i=0;i<buffer_size-1;i++){
            right_point_buffer[i] = right_point_buffer[i+1];
            right_length_buffer[i] = right_length_buffer[i+1];
        }
        right_point_buffer[buffer_size-1] = right_average_point;
        right_length_buffer[buffer_size-1] = right_average_length;   
    }    
    
    //print left camera result 
    
    for(int i=0;i<=127;i++){
        if( i >= left_average_point-left_average_length && i<=left_average_point)
            pc.printf(" ");
        else
            pc.printf("x");              
    }
    
    //print right camera result
    
    for(int i=0;i<=127;i++){
        if( i <= right_average_point+right_average_length && i>=right_average_point)
            pc.printf(" ");
        else
            pc.printf("x");   
    } 
    
    
    pc.printf("\n L_center : %d , R_center : %d \r\n", left_average_point,right_average_point);
    pc.printf("\r\n");  
    
    //PID -- P
    int error = 0;
    int turn = 0;
    int center = 55;
    float Kp = 0;
    float turn_angle = 0;
    int reference_point= 0;
   
    if(isLeftAllWhite && !isRightAllWhite){
        error = 40;
        turn = 90;
        reference_point = right_average_point;
    }
    else if(!isLeftAllWhite && isRightAllWhite){
        error = 40;
        turn = 90;
        reference_point = left_average_point;
    }
    else if(isLeftAllWhite && isRightAllWhite){
        error = 30;
        turn = 90;
        if(left_average_point < 40 && right_average_point < 40)
             reference_point = 0;
        else if(left_average_point > 70 && right_average_point > 70)
             reference_point = 90;
             
        //pc.printf("\n allllllllllllllllllllll   whiteeeeeeeeeeeeeee \n");
        
    }
    else if(!isLeftAllWhite && !isRightAllWhite){
         error = 50;
         turn = 90;
         center = 108;
         reference_point = (right_average_point + 108 + left_average_point)/2;
    }
    
    Kp = turn/error;
    
    //PID -- I
    /*for(int i=0;i<4;i++){
        error_buffer[i] = error_buffer[i+1];
    } 
    error_buffer[4] = reference_point - center;
    float integral = 0;
    float Ki = 1.1f;
    for(int i=0;i<5;i++){
        integral = (2/3)*integral + error_buffer[i];
    }    
    turn_angle = (int) (Kp*(reference_point - center) + Ki*integral);*/
    
    turn_angle = (int) (Kp*(reference_point - center));
    if(turn_angle >= 50)
        turn_angle = 50;
    if(turn_angle <= -50)
        turn_angle = -50;
    
    servo.set_angle(turn_angle);
  } 
      
         
          
  #endif   
          
    return 0; 
    
}