File content as of revision 18:88b083db7491:

#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
#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;

void run();

int main()
{
    pc.baud(115200);

    /*while(1) {
        if(tsi.readPercentage()>0.00011)
            break;
    }*/
    run();

    return 0;
}



void run()
{
    double motor, angle=0.0;
    double b_r_c;
    double PID_v;
    double error , P, I, D, kp, r_kp;
    double last_error=0.0 ,last_I=0.0, last_brc, brc_df;
    bool first_time=true, r_kp_neg=false;

    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(0.0);
        MotorB.rotate(0.0);
#endif

        b_r_c=(double)cam.black_centerR();
        //pc.printf("%d %d speed :%f  bk_center %f PID:%f \r\n",cam.de_v,cam.de_v2,motor,b_r_c,PID_v);

        //compute
        //stand at 65
        error=b_r_c-offset;
        brc_df=b_r_c-last_brc;
        last_brc=b_r_c;
        
        if(first_time==true){
                r_kp=0;
                first_time=false;
            }
        else{
                r_kp+=brc_df;
            }
            
        if(r_kp<0.0){
                r_kp*=-1.0;
                r_kp_neg=true;
            }
        kp=0.016/(1280-r_kp);
        if(r_kp_neg==true){
                r_kp_neg=false;
                r_kp*=-1.0;
            } 
                
        P=kp*error;

        I=last_I*2.0/3.0+error;
        last_I=I;
        I=(kp*0.02/0.04)*I;

        D=error-last_error;
        last_error=error;
        D=(kp*0.04/0.02)*D;

        PID_v=P;

        if(PID_v < 0.0) {
            PID_v*=-1.0;
            PID_v=0.077-PID_v;
        } else if(PID_v > 0.0)
            PID_v=0.077+PID_v;
        else
            PID_v=0.077;
        pc.printf("%lf %lf %lf\r\n",PID_v, r_kp, b_r_c);
        //pc.printf("%lf\r\n",angle);
        servo.set_angle(PID_v);

#ifdef task_ma_time
        task_pin=0;
#endif
    }
}