File content as of revision 0:e0e1b495278b:

//歩くだけ
#include "mbed.h"
#include "ColorSensor.h"
#include "TextLCD.h"
#include "Servo.h"
#include "HMC6352.h"

#include "main.h"

//センサの数
#define COLOR_NUM 3

//閾値
#define R_THR 60
#define G_THR 60
#define B_THR 60


//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);

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

//ColorSensor color = ColorSensor();
//Serial pc(USBTX, USBRX); // tx, rx

DigitalOut led[4] = {LED1,LED2,LED3,LED4};
DigitalOut air[2] = {p21,p22};

Timer color_t[3];


double proportional = 0;
uint16_t com_val = 0;


#define Convert_dekaruto(a) ((a+100.0)/2.0/100.0)

#define STRAIGHT 0.6;
#define SPIN 0.4;

//#define STRAIGHT 0.0;
//#define SPIN 1.0;


void move(int vl,int vs){
    double fut_R,fut_L,true_vs;
    
    true_vs = abs(vs)/SPIN;
    
    if(true_vs > 40){
        vl = 0;
        vs = 100*(vs/abs(vs));
    }
    
    fut_R = Convert_dekaruto((vl + vs));
    fut_L = Convert_dekaruto((vl - vs)/**1.4*/);
    
    fut_R = Convert_dekaruto(-50);
    fut_L = Convert_dekaruto(-50);
 
    servoR = fut_R;
    servoL = fut_L;
       
    //printf("R:%lf   L:%lf\n",fut_R,fut_L);
    //printf("R:%d   L:%d\n",(vl + vs),-(vl - vs));
}

void PidUpdate()
{   
    inputPID = (((int)(compass.sample() - ((207.0) * 10.0) + 5400.0) % 3600) / 10.0);
    //printf("%lf\n",inputPID);      
}

double vsOut(){
    double vs;
    vs = ((inputPID / 360 - 0.5) * 2 * 100) * -1;
    vs = vs * 8;
    
    if(vs/abs(vs) < 0){
        //vs *= 1.3;   
    }
    
    if(abs(vs) > 90)vs = 90*(vs/abs(vs));
    if(abs(vs) < 25) vs = 25*(vs/abs(vs));
    
    return vs;
}



//青から赤に反応したらジャンプ
//pidUpdataのせいでカラーセンサが動かない!!
int main()
{
    wait(3);

    unsigned R, G, B;
    unsigned threshold, t[3] = {0} ;
    double color_sum;
    unsigned redp[COLOR_NUM], greenp[COLOR_NUM], bluep[COLOR_NUM];
    //pc.baud(115200);
    air[0] = 0; air[1] = 1;
  
    int vl;
    double vs;
    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    pidUpdata.attach(&PidUpdate, PID_CYCLE);
  
  
  while(1)
  {
     
    color0.checkRGB(redp[0],greenp[0],bluep[0]);
    color1.checkRGB(redp[1],greenp[1],bluep[1]);
    color2.checkRGB(redp[2],greenp[2],bluep[2]);
    
    //lcd.cls();
    //pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
    /*
    lcd.locate(0,0);
    lcd.printf("R:%d G:%d B:%d", redp[0][0], greenp[0][0], bluep[0][0]);
    lcd.locate(0,1);
    lcd.printf("R:%d G:%d B:%d", redp[1][0], greenp[1][0], bluep[1][0]);
     */
     

     
    
    threshold = 0;
    for(int i=0; i<3; i++){
        if(bluep[i] >= B_THR){
            color_t[i].reset();
            color_t[i].start();
            t[i] = 0;
        }else if(redp[i] >= R_THR){
            t[i] = color_t[i].read_ms();
        }else{
            t[i] = 0;
        }
 
        if((t[i] <= 500) && (t[i] != 0)){
            threshold++;
        }
    }
    
    
    
    if(threshold >= 1){
            led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1;
            air[0] = 1;  air[1] = 0;
            wait(0.8);
            led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
            air[0] = 0;  air[1] = 1;
            wait(0.5);
    }else{
            led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
    }
    
    
    
    vl = -90;
    //vl = 0;
    
    vl = vl      * STRAIGHT ;
    vs = vsOut() * SPIN     ;
    
    vl *= 0.5;
    vs *= 0.3;
        
    move(vl,(int)vs); 
  }
}