File content as of revision 0:feefc7aaa114:

#include "mbed.h"
#include "ColorSensor.h"
#include "TextLCD.h"
#include "Servo.h"
#include "HMC6352.h"

#include "main.h"


void tic_sensor()
{
    Ultrasonic();
    
    colorUpdate();
    
    /*lcd.cls();
    lcd.locate(0,0);
    lcd.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[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;
}

////////////////////////////////////////超音波センサの////////////////////////////////////////
////////////////////////////////////////スイッチ的な関数//////////////////////////////////////
int ping_button(int ping,int button){
    static int continue_flag = 0;
    static int change_flag = 0;

    if(continue_flag == 0){
        if(ping <= PINR_THR){
            ping_t.start();
            continue_flag = 1;
        }
    }    
    
    if(continue_flag == 1){
        //agatterutoki
        if(ping <= PINR_THR){
            if(change_flag == 0){
                if(ping_t.read_ms() >= 300){
                    button = !button;
                    change_flag = 1;
                }
            }
        }
        //tatisagari
        if(ping >= (PINR_THR+200)){
            ping_t.stop();
            ping_t.reset();
            continue_flag = 0;
            change_flag = 0;
        }       
    }
    return button;    
}

////////////////////////////////////////カラーセンサの////////////////////////////////////////
////////////////////////////////////////補正プログラム////////////////////////////////////////
void rivisedate()
{
    unsigned long red = 0,green = 0,blue =0;
    static unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
    
    //最初の20回だけ平均を取る
    for (int i=0;i<=20;i++){
         color0.getRGB(R[0],G[0],B[0]);
         red       += R[0] ;
         green     += G[0] ;
         blue      += B[0] ;
         //pc.printf(" %d  %d\n",ptm(sum),sum);
    }
    
    rir = (double)green/ red ;
    rib = (double)green/ blue ;
}

void colorUpdate()
{
    double colorSum[COLOR_NUM];
    unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];

    color0.getRGB(R[0],G[0],B[0]);
    color1.getRGB(R[1],G[1],B[1]);
    color2.getRGB(R[2],G[2],B[2]);
    /*color3.getRGB(R[3],G[3],B[3]);
    color4.getRGB(R[4],G[4],B[4]);
    color5.getRGB(R[5],G[5],B[5]);*/
    
    for (int i=0; i<COLOR_NUM; i++){
        colorSum[i] = R[i]*rir + G[i] + B[i]*rib ;
        redp[i]   = R[i]* rir * 100 / colorSum[i];
        greenp[i] = G[i]      * 100 / colorSum[i];
        bluep[i]  = B[i]* rib * 100 / colorSum[i];
    }
}

////////////////////////////////////////ジャンププログラム////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////
uint8_t jumpcondition()
{
    uint8_t threshold = 0, t[3] = {0};
    
    //青から赤に0.5秒以内に反応したらジャンプ
    for(int i=0; i<COLOR_NUM; 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++;
        }
    }
    
    return threshold;
}

void jumping(uint8_t threshold)
{
    //超音波でジャンプのタイミング合わせる
    if(threshold >= 1){
            jump_t.reset();
            jump_t.start();
            while(ultrasonicVal[0] < 1700){
                led[0] = 1; led[1] = 1; led[2] = 0; led[3] = 0;
                air[0] = 1;  air[1] = 0;
                
                if(jump_t.read_ms() > 1000)break;
            }
            led[0] = 0; led[1] = 0; led[2] = 1; led[3] = 1;
            air[0] = 0;  air[1] = 1;
            wait(0.5);
    }else{
            led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
    }
}











int main()
{
    rivisedate();

    timer2.start();
    ping_t.start();
    
        
    //unsigned R, G, B;
    int vl;
    double vs;
    uint8_t button, state=0;
    
    //pc.baud(115200);
    air[0] = 0; air[1] = 1;
  
    
    //compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    //pidUpdata.attach(&PidUpdate, PID_CYCLE);
    interrupt0.attach(&tic_sensor, 0.05/*sec*/);//0.04sec以上じゃないとmain動かない
  
  while(1)
  {
    
    
    
    pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
    /*
    lcd.cls();
    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]);
     */
    //pc.printf("%d\n", ultrasonicVal[0]);
    
    
    jumping(jumpcondition());
    
    
    
    button = ping_button(ultrasonicVal[1],button);
        
    if(button){
        state = GO;
    }else{
        state = STOP;
    }
    
    
    
    
    
    if(state == GO){
        vl = -90;//led[0] = 1; led[1] = 1;
    }else if(state == STOP){
        vl = 0;//led[0] = 0; led[1] = 0;
    }
    
    vl = vl      * STRAIGHT ;
    vs = vsOut() * SPIN     ;
    
    vl *= 0.5;
    vs *= 0.3;
        
    move(vl,(int)vs); 
  }
}