File content as of revision 0:1be472d79ae9:

#define RATE 0.1
#define MINIMUM -1440
#define MAXIMUM 1440 

#define PID_CYCLE   0.03    //s
#define COMPASS_CYCLE   0.1

#define P_GAIN  1.2   //0.78   //zensin 1.5
#define I_GAIN  1000000000.0   
#define D_GAIN  0.0   //0.009

#define PID_BIAS    0.0
#define REFERENCE   0

#define OUT_LIMIT 1000

extern double ultrasonicValue[4];
extern uint16_t ultrasonicVal[4];
extern void Ultrasonic(void);
Timer timer2;
Timer ping_t;

HMC6352 compass(p28/*sda*/, p27/*scl*/);
Ticker compassUpdata;

PID pid(P_GAIN,I_GAIN,D_GAIN, RATE); 
PID controller(1.0, 0.0, 0.0, RATE);
Ticker pidUpdata;

Servo servoR(p23);
Servo servoL(p24);

DigitalIn  swR(p29);
DigitalIn  swL(p30);

DigitalOut led1(LED1); 
DigitalOut led2(LED2); 

QEI wheel1 (p18, p17, NC, 624);
QEI wheel2 (p8, p7, NC, 624);

#define Convert_dekaruto(a) ((a+1000.0)/2.0/1000.0)
#define CYCLE 705.0

enum{
    WAIT,
    STRAIGHT,
    TURN,
    COMP
};