File content as of revision 0:e6bb0bcba274:

///////////////////////////////////////OGATA//////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////
//センサの数
#define COLOR_NUM 3

//閾値
#define R_THR 65
#define G_THR 65
#define B_THR 65
#define PINR_THR 2000


enum{
    GO,
    STOP
};

//TextLCD lcd(p30, p29, p28, p27, p26, p25, TextLCD::LCD20x4); // rs, e, d4-d7
ColorSensor color0(p20, p17, p18, p19, 10);
ColorSensor color1(p16, p13, p14, p15, 10);
ColorSensor color2(p12, p9, p10, p11, 10);
Serial pc(USBTX, USBRX);    // tx, rx 
DigitalOut led[4] = {LED1,LED2,LED3,LED4};
DigitalOut air[2] = {p7,p8};


Timer color_t[3];
Timer jump_t;
Ticker interrupt0;


void rivisedate ();
void colorUpdate ();
uint8_t ptm(unsigned sum);


double proportional = 0;
uint16_t com_val = 0;
unsigned redp[COLOR_NUM], greenp[COLOR_NUM], bluep[COLOR_NUM];
double rir,rib ;







///////////////////////////////////////YUSUKE/////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////
#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(p25);
Servo servoL(p26);

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

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

QEI wheel1 (p24/*A*/, p23/*B*/, NC, 624);
QEI wheel2 (p22, p21, NC, 624);

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

enum{
    WAIT,
    STRAIGHT,
    TURN,
    COMP
};


double vl = 0;
double vc = 0;
double vs = 1;
int clwise = 0;
int anclwise = 0;

int enkoda1;
int enkoda2;

int process = 0;
int vc_count = 0;
int mode_comp = 0;

int inputPID = 0;