File content as of revision 21:5f7efc1ca8ad:

#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 L_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(0);
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=127; i>=64; i--) {
            if(i==127)
                pc.printf("X");
            else if(i==64)
                pc.printf("X");
            else
                pc.printf("%c", cam.sign_line_imageL[i]);
        }
        pc.printf("      ||        ");
#endif
#ifdef R_eye
        for(int i=64; i>=0; i--) {
            if(i==64)
                pc.printf("X");
            else
                pc.printf("%c", cam.sign_line_imageR[i]);
        }
        pc.printf("    ");
        pc.printf("\r\n");
#endif
#endif

#ifdef motor_on
        motor=pot1.read();
        MotorA.rotate(motor);
        MotorB.rotate(motor);
        //pc.printf("%f\r\n",motor);
#endif

        b_r_c=(double)cam.black_center();
        
        PID_v=cam_to_M_ctrlr.compute(b_r_c,64);//set_angle()
        pc.printf("\r\n");
        //pc.printf("b_r_c:  %f\r\n",b_r_c);
        //pc.printf("angle:  %f\r\n",PID_v);
         //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,center,PID_v);
            //center range 0~128
            last_error = error;
        //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);
        servo.set_angle(PID_v);

#ifdef task_ma_time
        task_pin=0;
#endif
    }
}