File content as of revision 23:d6d4e8adf7fe:

#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;
    int centerR,centerL,center;
    double PID_v;
    double P, I, D, kp = 0.0004, r_kp;
    int errorR,errorL,error,last_error;
    double 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
        pc.printf("X");
        for(int i=122; i>=44; 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=84; i>=6; i--) {
            if(i==64)
                pc.printf("X");
            else
                pc.printf("%c", cam.sign_line_imageR[i]);
        }
        pc.printf("X");
        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

        centerR=cam.black_centerR();
        centerL=cam.black_centerL();
        center = cam_to_M_ctrlr.getCenter(centerL,centerR);
        
        PID_v = (double)cam_to_M_ctrlr.compute(centerL,centerR,64);
        //servo.set_angle(PID_v);
        servo.set_angle(PID_v);
        pc.printf("centerL:   %d       centerR:   %d       center:   %d",centerL,centerR,center);
        pc.printf("\r\n");
        for(int i=118; i>=10; i--){
            if(i==64)
                pc.printf("X");
            else if(i>=center-4 && i<=center+4)
                pc.printf(" ");
            else
                pc.printf("O");
        }
        pc.printf("\r\n");
#ifdef task_ma_time
        task_pin=0;
#endif
    }
}