File content as of revision 0:e6bb0bcba274:

#include "mbed.h"
#include "PID.h"
#include "QEI.h"
#include "Servo.h"
#include "HMC6352.h"
#include "ColorSensor.h"
#include "TextLCD.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 PidUpdate(){
    static int state = WAIT,past_state = WAIT,next_state = WAIT;

    if(!mode_comp){
        if(vl && !vs){
            state = STRAIGHT;
        }
        if(vs){
            state = TURN;
        }
    }
    if((state != past_state)){
        mode_comp = 1;
    
        if(process == 0){
            if(abs(enkoda1) > 180 && abs(enkoda1) < 240){
                process++;
            }    
        }
        if(process == 1){
            if(state == STRAIGHT){
                vl = 10;
                vs = 0;
            }
            if(state == TURN){
                vl = 0;
                vs = 10;
            }        
            if(abs(vc) < 60){
                vc_count++;
            }
            if(vc_count > 2){
                vc_count = 0;
                mode_comp = 0;
                process = 0;
            }
        }
    }

    enkoda1 = (int)(((double)wheel1.getPulses()/CYCLE) * 360.0);
    enkoda2 = (int)(((double)wheel2.getPulses()/CYCLE) * 360.0);

    if((!vs)&&(mode_comp == 0)){
        pid.setProcessValue(wheel1.getPulses() + wheel2.getPulses());
    }else if((vs)&&(mode_comp == 0)){
        pid.setProcessValue(wheel1.getPulses() - wheel2.getPulses());
    }
    
    if((!vs)&&(mode_comp)){
        pid.setProcessValue(wheel1.getPulses() + wheel2.getPulses());
    }else if((vs)&&(mode_comp)){
        pid.setProcessValue(wheel1.getPulses() - wheel2.getPulses());
    }
        
    vc = (int)pid.compute();
           
    printf("vc:%lf\n",vc);
    
    double fut_R = 0.5,fut_L = 0.5;
    
    if(abs(vc) > 250){
        vc = 0;
    }

    int vlR = 0;
    int vlL = 0;
    int vcR = vc;
    int vcL = vc;
    
    vlR = -vs;
    vlL =  vs;
    
    vl *= 0.5;
    vc *= 0.5;
    
    vlR *= 0.4;
    vlL *= 0.4;
    
    vcR *= 0.6;
    vcL *= 0.6;

    
    if(!vs){   
        if(vl > 0){
            fut_R = Convert_dekaruto((int)(vl - vc));
            fut_L = Convert_dekaruto((int)(vl * 0.95 + vc)); 
        }
        if(vl < 0){
            vc *= -1; 
            fut_R = Convert_dekaruto((int)(vl * 0.95 + vc));
            fut_L = Convert_dekaruto((int)(vl - vc)); 
        }
    }else{
        if(vlR < 0){
            vcR *= -1;  
        }
        
        if(vlL < 0){
            vcL *= -1;  
        }
        if(vs > 0){
            fut_R = Convert_dekaruto((int)(vlR * 0.9 + vcR));
            fut_L = Convert_dekaruto((int)(vlL * 0.65 - vcL));
        }
        
        if(vs < 0){
            fut_R = Convert_dekaruto((int)(vlR * 0.65 - vcR));
            fut_L = Convert_dekaruto((int)(vlL * 0.9+ vcL));
        }
    }
    
    servoR = fut_R;
    servoL = fut_L;
    
    if(!mode_comp){
        past_state = state;
    }    
}

void CompassUpdate(){
    inputPID = (((int)(compass.sample() - (/**/180 * 10.0) + 5400.0) % 3600) / 10.0);
}

double vssOut(){
    double vss;
    vss = ((inputPID / 360.0 - 0.5) * 2.0 * 1000.0) * 1.0;
    
    if(abs(vss) < 10){
        vss = 0;
    }
    
    vss *= 3.0;
    
    
    if((vss)&&(abs(vss) < 500)){
        vss += (vss/abs(vss)) * 500;
    }
        
    if(abs(vss) > 1000){
        vss = 1000 * (vss/abs(vss));
    }
    
    return vss;
}

void move(int vll,int vss){
    if(!swR){
        wheel1.reset();
    }
    
    if(!swL){
        wheel2.reset();
    }

    vl = vll;
    vs = vss;
}


////////////////////////////////////////超音波センサの////////////////////////////////////////
////////////////////////////////////////スイッチ的な関数//////////////////////////////////////
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() >= 150){
                    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;
    }
}


uint8_t robostop()
{
    if(bluep[1] >= B_THR){
        return 1;
    }else{
        return 0;
    }
}











int main() {
    rivisedate();
    wait(3);
    
    timer2.start();
    ping_t.start();

    pid.setInputLimits(MINIMUM,MAXIMUM);
    pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
    pid.setBias(PID_BIAS);
    pid.setMode(AUTO_MODE);
    pid.setSetPoint(REFERENCE);
    
    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    compassUpdata.attach(&CompassUpdate, COMPASS_CYCLE);
    
    swR.mode(PullUp);
    swL.mode(PullUp);
        
    air[0] = 0; air[1] = 1;
 
    int setR = 0,setL = 0;
    int vll = 0,vss = 0;
    
    servoR = 0.595;
    servoL = 0.59;
    
    while(1){
        if(!swR){
            setR = 1;
        }
        
        if(!swL){
            setL = 1;
        }
        
        if(setR){
            servoR = 0.5;    
        }
        
        if(setL){
            servoL = 0.5;    
        }
        
        if(setR && setL){
            break;
        }
       
        wait(0.1);
    }
    
    wheel1.reset();
    wheel2.reset();
    
    interrupt0.attach(&tic_sensor, 0.05/*sec*/);//0.04sec以上じゃないとmain動かない
    pidUpdata.attach(&PidUpdate, PID_CYCLE);
    
    uint8_t button, state=0;

    while(1) {
        vll = 0;
        vss = 0;
    
        pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
        jumping(jumpcondition());
    
    
    
        button = ping_button(ultrasonicVal[1],button);
            
        if(button){
            state = GO;
        }else{
            state = STOP;
        }
        
        
        
        
        
        if(state == GO){
            vll = 700;//led[0] = 1; led[1] = 1;
        }else if(state == STOP){
            vll = -700;//led[0] = 0; led[1] = 0;
        }
           
        move(vll,vss); 
    }
}