File content as of revision 15:7f21c08be164:

#include "mbed.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.027, middle = 0.038, right = 0.057;
    double error, turn, last_turn = 0.0, avg = middle;
    
    double last_error = 0;
    double Kp = 0.000327, Ki = 0.0001;
    int black_centerR, black_centerL, center, times = 0;
    int flag = 0, sp = 64;
    int t = 0;
    double last = 0.0;
    double avgg = middle,last_avgg = middle, next_avgg = middle;
    double v = 0.5;
    
    //char psudo_line[128];
    
#ifdef Debug_cam_uart
     pc.baud(115200);
     
     
  while(1){   
        
        cam.read();

        //MotorA.rotate(0.3);
        //MotorB.rotate(0.3);
        
        black_centerL = cam.black_centerL();
        black_centerR = cam.black_centerR();
        
        //center = (black_centerL + black_centerR) / 2;
        
        /*while(1){
            for(double i = 0.025; i < 0.058; i+= 0.001){
                servo.set_angle(0.038);
                wait_ms(1000);
            
            }
        }*/

        
        
        //catch little error
        if(black_centerL > 64 && black_centerR > 64){

            center = (black_centerL <= black_centerR) ? black_centerL : black_centerR;

        } else if(black_centerL > 64 && black_centerR < 64){
            
            center = (black_centerL-64 <= 64-black_centerR) ? black_centerL : black_centerR;


        } else if(black_centerL < 64 && black_centerR > 64){

            center = (64-black_centerL <= black_centerR-64) ? black_centerL : black_centerR;

        } else if(black_centerL < 64 && black_centerR < 64){

            center = (black_centerL >= black_centerR) ? black_centerL : black_centerR;

        } else{

            center = 64;
        }



        if(black_centerL == 118 && black_centerR == 10){//no line flag = 0
            
            error = 0;
            //turn = avg;
            if(avgg < 0.038)
                turn = 0.026;
            else 
                turn = 0.054;
            //else 
              //  turn = middle;
            flag = 0;
            
        } else if (black_centerL == 118 && black_centerR != 10){//no left line turn right flag = 1
            
            sp = 30;
            if(flag == 2){

                sp = 64;

            }

            error = sp - black_centerR;
            turn = middle + Kp * error;
            flag = 1;
            last_error = (1/3)*last_error + error;
        
        } else if (black_centerL != 118 && black_centerR == 10){//no right line turn left flag = 2
            
            sp = 90;

            if(flag == 1){

                sp = 64;

            }

            error = 90 - black_centerL;
            turn = middle + Kp * error;
            
            flag = 2;
            last_error = (1/3)*last_error + error;
            
        } else {//flag = 3

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

            if(flag == 1){

                if(black_centerL < 100){
                    servo.set_angle(avg-0.004);
                    turn = avg;
                }

            } else if(flag == 2){

                if(black_centerR > 28){
                    servo.set_angle(avg+0.004);
                    turn = avg;
                }
                
            }
            flag = 3;
        }

        last_turn += turn;
        last += turn;
        times++;
        t++;

        if(times == 5){
            
            avg = last_turn / times;
            if(avg < 0.042 && avg > 0.034){
                v = 0.5;
            }
            else{
                v = 0.18;
            }
            MotorA.rotate(v);
            MotorB.rotate(v);
            servo.set_angle((avg*6 + next_avgg)/7);
            //servo.set_angle(avg);
            times = last_turn = 0;
            
        }
        if(t == 1000){
            last_avgg = avgg;
            avgg = last / t;
            next_avgg = 2*avgg -last_avgg;
            t = 0;
            last = 0.0;
        }
       
        /*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   psudo_line: %d avg: %lf flag: %d  avgg: %lf\r\n", black_centerL, black_centerR, center, avg, flag,avgg);
        
        
        
         //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("ang :%d\r\n ",( (64.0-center) /64.0  )*90);
   //--------------------------------------------            


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

       
         
         
     
     
  #endif   
       
     
     
     
     
     
     
     
     
     
     
     
    
    
    
    
    return 0;
    
    
}