File content as of revision 8:089b778962c4:

#include "mbed.h"
#include "rtos.h"
#include "servo_api.h"
#include "camera_api.h"
#include "motor_api.h"
#include "TSISensor.h"
#include "pot.h"
#define Debug_cam_uart
#define buffer_size  6
#define e_buffer_size 50
//os
Mutex stdio_mutex;
BX_pot pot1('1');
TSISensor tsiSensor;

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

int left_point_buffer[buffer_size] = {0,0,0,0,0,0};
int right_point_buffer[buffer_size] = {0,0,0,0,0,0};
int error_buffer[e_buffer_size] ={0};
float angle_buffer[50]={0};
int state = 0; //0: no line, 1:right line, 2:left line
int last_state = 0;
float turnAngle = 0.073;
float Kp = 0.0;


void servo_thread(void const *args){
    while(1){
        for(int i=0;i<49;i++){
         angle_buffer[i] = angle_buffer[i+1];
        }
        angle_buffer[49] = turnAngle;
        
        float average_angle=0;
        for(int i=25;i<50;i++){
            average_angle=average_angle+angle_buffer[i];
        }
        average_angle = average_angle/25;
        float speed_error = average_angle - 0.073;
        int Km_speed = 0;;
        if(speed_error<0)
            speed_error = speed_error*(-1);
        
        if(speed_error <= 0.004 || speed_error >= 0.01){
            Km_speed = -13;
        }
        else{
            Km_speed = -7; 
        }
        
        float speed = Km_speed*speed_error; 
        int int_max =  pot1.read()*10;
        float max = int_max/(10.0);
        if(max+speed<0)
            speed = 0.2-max;
        motorA.rotate(max+speed);
        motorB.rotate(max+speed);
        
        //pc.printf("\r\n Turn Angle = %f \r\n",turnAngle );
        servo.set_angle(turnAngle);
        Thread :: wait(30);
    }
}

float find_error(){
    cam.read();
    int left_target_point = 0;
    int left_average_point = 0;  
    int left_black_count = 0;
    bool isLeftAllWhite = true;
    for(int i=117;i>=10;i--){
        if(cam.sign_line_imageL[i] == ' '){
            left_black_count++;
            if(left_black_count == 10){
                left_target_point = i;
                isLeftAllWhite = false;
                break;
            }            
        }
        else{
            left_black_count = 0;
        }
    }
    
    if(isLeftAllWhite){
       //do not update the data
        //left_average_point =  (left_point_buffer[0] + left_point_buffer[1] + left_point_buffer[2] + left_point_buffer[3] +left_point_buffer[4]+left_point_buffer[5])/buffer_size;     
        left_average_point = left_point_buffer[5];
    }
    else{
       //average the history data
       //left_average_point =  (left_point_buffer[0] + left_point_buffer[1] + left_point_buffer[2] + left_point_buffer[3] +left_point_buffer[4]+left_point_buffer[5]+ left_target_point)/(buffer_size+1);
       left_average_point = left_target_point;
    }
    
    
    //====== find right line's left edging ======
    int right_target_point = 0;
    int right_average_point =  0;
    int right_black_count = 0;
    bool isRightAllWhite = true;
    for(int i=10;i<118;i++){
        if(cam.sign_line_imageR[i] == ' '){
            right_black_count++;
            if(right_black_count == 10){
                right_target_point = i;
                isRightAllWhite = false;
                break;
            }            
        }
        else{
            right_black_count = 0;
        }
    }
    
    /*for(int i=118;i>=10;i--){
        if(i<left_average_point){
         pc.printf(" ");
        }
        else
            pc.printf("O");
    }*/

    


    if(isRightAllWhite){
        //do not update the buffer
        //right_average_point =  (right_point_buffer[0] + right_point_buffer[1] + right_point_buffer[2] + right_point_buffer[3] + right_point_buffer[4] + right_point_buffer[5])/buffer_size;
        right_average_point = right_point_buffer[5];
    }
    else{
        //average the history data
        //right_average_point =  (right_point_buffer[0] + right_point_buffer[1] + right_point_buffer[2] + right_point_buffer[3] + right_point_buffer[4] + right_point_buffer[5] + right_target_point)/(buffer_size+1);
        right_average_point = right_target_point;
    }    
    /*
    for(int i=10;i<118;i++){
        if(i>=right_average_point){
            pc.printf(" ");
        }
        else
            pc.printf("O");
    }
        pc.printf("\r\n");*/
    //update buffer
    for(int i=0;i<buffer_size-1;i++){
        right_point_buffer[i] = right_point_buffer[i+1];
        left_point_buffer[i] = left_point_buffer[i+1];
    }
    
    left_point_buffer[buffer_size-1] = left_average_point;
    right_point_buffer[buffer_size-1] = right_average_point;
    
    int center = 0;
    int reference_point = 0;
    if(isLeftAllWhite && !isRightAllWhite){
        state = 1;
        Kp = 0.0004;
        center = 102;
        reference_point = right_average_point;
    
    }
    else if(!isLeftAllWhite && isRightAllWhite){
        state = 2;
        Kp = 0.0004;
        center = 20;
        reference_point = left_average_point;
    }
    else if(isLeftAllWhite && isRightAllWhite){
        state = 0;
        Kp = 0.0004;
        center = 64;
        //straight line
        float average_angle;
        for(int i=0;i<50;i++){
            average_angle = average_angle + angle_buffer[i];
        }
        average_angle = average_angle/50;
        
        if(average_angle > 0.064 && average_angle < 0.08){
            Kp = 0;   
        }
        else{
            if(left_average_point <= 40 && right_average_point <= 40)
                reference_point = 15;
            else if(left_average_point >= 70 && right_average_point >= 70)
                reference_point = 95;    
        }
    }
    else if(!isLeftAllWhite && !isRightAllWhite){
         Kp = 0.0002;
         
         if(128  -  left_average_point  >=   right_average_point)
         {  
            state = 1;
            center = 95;
            reference_point = right_average_point;
         }
         else{
            state = 2;
            center = 15;
            reference_point = left_average_point;
        }
    }   
    

    return reference_point - center;
}


int main() {
    
    while (1)
    {
        if (tsiSensor.readPercentage() > 0.00011) 
        {
            Thread :: wait(2000);
            break;
        }
    }

    servo.set_angle(0.073);

#ifdef Debug_cam_uart
     pc.baud(115200);
     
    
     
     
     motorA.rotate(0.33);
     motorB.rotate(0.33);
      Thread th_s(servo_thread);
     while(1){        
        if(last_state != state){
            for(int i=0;i<e_buffer_size - 1;i++){
                error_buffer[i] = 0;
            }
        }
        else{
            for(int i=0;i<e_buffer_size - 1;i++){
                error_buffer[i] = error_buffer[i+1];
            }  
        }
        error_buffer[e_buffer_size-1] = find_error();
    
        float past_error = 0;
        for(int i= 0;i<e_buffer_size;i++){
            past_error = (past_error*2)/3 + error_buffer[i];
        }
        past_error = past_error/e_buffer_size;
        float future_error =  (error_buffer[e_buffer_size-1] - error_buffer[0])/e_buffer_size + error_buffer[e_buffer_size-1];
    
        stdio_mutex.lock();
        turnAngle = 0.073 + Kp*(error_buffer[e_buffer_size-1]
                          + (1*past_error)/9
                          + (4*future_error)/9);
        stdio_mutex.unlock();
        last_state = state;
  
  
       // pc.printf("%f \r\n",pot1.read());
  
  
        Thread :: wait(5);
  }   

          
  #endif   
          
    return 0; 
    
}