File content as of revision 0:4c7c138f3891:

//カラーセンサ6つ同時に動かすプログラム
//大会用ロボ
//ジャンプのプログラムも入ってる
//10月17日に使う
#include "mbed.h"
#include "ColorSensor.h"
#include "Servo.h"

#include "main.h"




int cAve,roboF, jumI;

void tic_sensor()
{
    colorUpdate(1);
    roboF = robotFront();
    jumI = jumpInstruction(roboF);
    //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\n", redp[0], greenp[0], bluep[0]);
    */
}



////////////////////////////////////////カラーセンサの////////////////////////////////////////
////////////////////////////////////////更新と補正プログラム///////////////////////////////////
/*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(uint8_t mode)
{
    double colorSum[COLOR_NUM];
    unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];


    for (int i=0; i<COLOR_NUM; i++){
        R[i] = 0;
        G[i] = 0;
        B[i] = 0;
        redp[i]   = 0;
        greenp[i] = 0;
        bluep[i]  = 0;
    }
    
    //カラーセンサの切り替え    
    if(mode == 1){
        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];
    }*/ 
 
    for (int i=0; i<COLOR_NUM; i++){
        colorSum[i] = R[i]*0.65 + G[i] + B[i] *1.3;
        redp[i]   = R[i]*0.65 * 100 / colorSum[i];
        greenp[i] = G[i]      * 100 / colorSum[i];
        bluep[i]  = B[i]*1.3 * 100 / colorSum[i];
    }
}


uint16_t moving_ave(uint8_t num, uint16_t data)//移動平均的な
{
    uint8_t threshold = 0;
    static uint16_t tmp[COLOR_NUM][10] = {{0}};
    //static uint32_t sum[COLOR_NUM] = {0};
    
    //sum[num] -= tmp[num][9];
    //sum[num] += data;
    
    for(uint8_t i=9; i>=1; i--){
        tmp[num][i] = tmp[num][i-1];
    }
    tmp[num][0] = data;
    
    for(uint8_t i=0; i<=9; i++){
        if(tmp[num][i] >= B_THR){
            threshold++;
        }
    }
    
    return threshold;
}

////////////////////////////////////////ロボの状態///////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////
uint8_t robotFront()//縄回しロボの正面にいるか
{
    uint8_t threshold = 0;
    
    for(int i=0; i<COLOR_NUM; i++){
        if(moving_ave(i, bluep[i]) >= 8) threshold++;
    }
    
    
    /*for(int i=0; i<COLOR_NUM; i++){
        cAve = moving_ave(i, bluep[i]);
        if(cAve >= 8) threshold++;
    }*/

    if(threshold >= 1){
        return 1;
    }else{
        return 0;
    }
}

uint8_t jumpInstruction(uint8_t front)//ジャンプ命令
{
    uint8_t threshold = 0;
    
    if(front == 1){
        for(int i=0; i<COLOR_NUM; i++){
            if(redp[i] >= R_THR) threshold++;
        }
    }
    
    if(threshold >= 1){
        return 1;
    }else{
        return 0;
    }
}


uint8_t robotoStop()
{
    uint8_t threshold = 0;

    for(int i=0; i<COLOR_NUM; i++){
        if(bluep[i] >= B_THR) threshold++;
    }
    
    if(threshold >= 1){
        return 1;
    }else{
        return 0;
    }
}


////////////////////////////////////////ジャンププログラム////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////
/*
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;
    }
}*/

/*void jumpAction(uint8_t threshold)
{
    if(sw1){//フォトインタラプタがオフ
        myservo1 = 0.3; led[0] = 0;led[1] = 1; led[2] = 0; led[3] = 0;
    }else{//フォトインタラプタがオン
        myservo1 = 0.5; led[0] = 1;led[1] = 0; led[2] = 0; led[3] = 0;
    }
        
    if(threshold){
        led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1;
        myservo1 = 0.3;
        wait(0.2);
    }else{
        //led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
    }
    
}*/






#define SEND_DATA_NUM       7
#define RECEIVE_DATA_NUM    7

#define KEYCODE             120

Serial device(p28,p27);

static uint8_t SendData0[SEND_DATA_NUM];

void dev_tx()
{
    static uint8_t count = 0;
    
    SendData0[0] = KEYCODE;

    device.putc(SendData0[count]);
     
    count++;
    
    if(count > SEND_DATA_NUM){
        count = 0;
    }
    
    led[4] = 1;
}





int main() {
    //init

    /*device.baud(9600);
    device.printf("START");
    device.attach(&dev_tx,Serial::TxIrq);*/

    //rivisedate();
    wait(3);

    interrupt0.attach(&tic_sensor, 0.1/*sec*/);//0.04sec以上じゃないとmain動かない?
    //sw1.mode(PullUp);
    //init end

    while(1) {
        SendData0[1] = jumI;    
        
        pc.printf("R:%d G:%d B:%d %t moving_ave():%d %t robotFront():%d %t jumpInstruction():%d\n", redp[2], greenp[2], bluep[2], cAve, roboF, jumI);
        
        //jumpAction(jumI);
    }
}