File content as of revision 10:9f0ce6ba7663:

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


#define Debug_cam_uart
#define R_eye
#define motor_on 
#define Pcontroller
#define task_ma_time
#define offset 65
#define kp 1
#define ki 0.1
#define kd 1
#include "TSISensor.h"

Serial pc(USBTX, USBRX);
BX_servo servo; 
BX_camera cam;
BX_motor MotorA('A');
BX_motor MotorB('B');
BX_pot pot1('1');            // 90/30=3
BX_pot pot2('2');
PID cam_to_M_ctrlr(10.0,118.0,0.06,0.11,(0.104/30),0.0,0.0,10);
DigitalOut task_pin(PTD1);
TSISensor tsi;

double Computes(double, double, double);
void run();

int main(){
    pc.baud(115200);
    
    while(1){    
        if(tsi.readPercentage()>0.00011)
           break;
    }
    run();
    
    return 0;
    }

double Computes(double b_r_c, double* last_I, double* last_error){
    //stand at 100
    double error , P, I, D, PID;
    
    error=b_r_c-100;
    P=kp*error;
        
    I=*last_I*2.0/3.0+error;
    *last_I=I;
    I=ki*I;
    
    D=error-*last_error+error;
    *last_error=error;
    D=kd*D;
    
    PID=P+I+D;
    
    if(PID<0.0){
        PID*=-1;
        PID=0.085-(PID*0.025);
        }
    else if(PID>0.0)
        PID=0.085+(PID*0.025);
    else
        PID=0.085;
    
    return PID;
    }

void run(){
     double motor;
     double b_r_c;
     double PID_v;
     
     while(1){   
        #ifdef task_ma_time
            task_pin=1;
        #endif   
        cam.read();  
        #ifdef Debug_cam_uart
            #ifdef L_eye  
            for(int i=0;i<128;i++){
                if(i==64)
                    pc.printf("X");
                else          
                    pc.printf("%c", cam.sign_line_imageL[i]);
            }
            pc.printf("           ||             ");
            #endif
            #ifdef R_eye
                for(int i=128;i>=0;i--){
                    if(i==64)
                        pc.printf("X");             
                    else if(i<10)
                        pc.printf("-");
                    else if(i>117)
                        pc.printf("-");
                    else          
                        pc.printf("%c", cam.sign_line_imageR[i]);
                }
                pc.printf("\r\n");
            #endif
        #endif   
     
        #ifdef motor_on 
            motor=pot1.read();
            MotorA.rotate(motor);
            MotorB.rotate(motor);
        #endif
       
        b_r_c=(double)cam.black_centerR();

        //PID_v=cam_to_M_ctrlr.compute(b_r_c,15.0);
        pc.printf("%f %d %d speed :%f  bk_center %f PID:%f \r\n",cam_to_M_ctrlr.de_v,cam.de_v,cam.de_v2,motor,b_r_c,PID_v);
         
        //servo.set_angle(PID_v);
        
        
        
        //PID_v = Computes(b_r_c, &last_I, &last_error);
        //compute
        //stand at 100
        double error , P, I, D;
        double last_error=0.0, last_I=0.0;
        
        if(error<20.0 && error>0.0)
            PID_v=0.0;
        else{
            error=b_r_c-offset;
            P=kp*error;
                
            I=last_I*2.0/3.0+error;
            last_I=I;
            I=ki*I;
            
            D=error-last_error;
            last_error=error;
            D=kd*D;
            
            PID_v=P+I+D;
        }
        
        if(PID_v < 0.0){
            PID_v*=-1;
            PID_v=0.085-(PID_v/125*0.025);
            }
        else if(PID_v > 0.0)
            PID_v=0.085+(PID_v/125*0.025);
        else
            PID_v=0.085;
        
        servo.set_angle(PID_v);
        
        #ifdef task_ma_time
            task_pin=0;
        #endif 
    } 
}