File content as of revision 85:dd18a2d79956:

#ifndef AUTOMODE_H
#define AUTOMODE_H

/***PID Controller***/
//PID velocity_controller(36.0,5274.0 ,0.0,RATE);
//PID direction_controller(36.0,3.0,0.0,RATE);
PID velocity_controller(9.0,5274.0,0.0,RATE);
PID direction_controller(36.0,3.0,0.0,RATE);

#ifdef IM920
/***IM920 correspondence***/
void autoIM920() {
    if(b==7){  /*mode change*/
        if(edge7) {
            edge7=0;
            autoflag=0;
            Indicator4=0;
            IndicatorAuto=1;
        }
    }
    else if(b==6){ /*start*/
        if(edge6){
            edge6=0;
            resetState();
            flagf=1;
            targ_velocity=speed;
//            sendData(7,70);
            sendData(5,69);
            wait(0.1);
            sendData(4,69);
            stateR = 69;
            stateL = 69;
            step   = 0;
            CStep  = 1;
        }
    }
    else if(b==5){ /*middle start*/
        if(edge5){
            edge5=0;
            resetState();
            flagf = 1;
            targ_velocity=speed;
            sendData(7,61);
            /*sendData(4,61);
            wait(0.1);
            sendData(5,58);*/
            stateR = 61;
            stateL = 61;
            step   = 5;
            CStep  = 6;
        }
    }   
    else if(b==8){  /*opponents start*/
        if(edge8){
            edge8=0;
            resetState();
            flagf = 1;
            targ_velocity = speed;
            sendData(7,70);
            stateR = 70;
            stateL = 70;
            step   = 15;
            CStep  = 15;
        }
    }
    else if(b==9){  /*stop*/
        Motor_swing=0;
//        sita=PI/4.0,x=0.0,y=0.0;
        targ_velocity=0.0;
#ifdef BLUE
        targ_sita=PI/4.0;
#else   
        targ_sita=-PI/4.0;
#endif
        velocity_controller.reset();
        direction_controller.reset();
    }
    else if(b==1){  /*L down*/
        if(edge1){
            edge1=0;
            if(stateL!=1) stateL--;
            sendData(4,stateL);
        }
    }
    else if(b==2){  /*L up*/
        if(edge2){
            edge2=0;
            if(stateL!=MAX_VALUE) stateL++;
            sendData(4,stateL);
        }
    }
    else if(b==3){  /*R down*/
        if(edge3){
            edge3=0;
            if(stateR!=1) stateR--;
            sendData(5,stateR);
        }
    }
    else if(b==4){  /*R up*/
        if(edge4){
            edge4=0;
            if(stateR!=MAX_VALUE) stateR++;
            sendData(5,stateR);
        }
    }
    if(a2){
        skip = 1;
    }
    else if(!a2){
        skip = 0;
    }
    if(b!=1) edge1=1;
    if(b!=2) edge2=1;
    if(b!=3) edge3=1;
    if(b!=4) edge4=1;
    if(b!=5) edge5=1;
    if(b!=6) edge6=1;
    if(b!=7) edge7=1;
    if(b!=8) edge8=1;
}
#else
void autoPS3(){
    if(circle){  /*mode change*/
        if(edge_circle){
            edge_circle=0;
            autoflag=0;
            Indicator4=0;
            IndicatorAuto=1;
        }
    }
    else if(up){  /*start*/
        if(edge_up){
            edge_up=0;
            resetState();
            flagf=1;
            targ_velocity=speed;
//            sendData(7,70);
            sendData(5,69);
            wait(0.1);
            sendData(4,69);
            stateR = 69;
            stateL = 69;
            step   = 0;
            CStep  = 1;
        }
    }
    else if(right){  /*middle start*/
        if(edge_right){
            edge_right=0;
            resetState();
            flagf = 1;
            targ_velocity=speed;
            sendData(7,61);
            /*sendData(4,61);
            wait(0.1);
            sendData(5,58);*/
            stateR = 61;
            stateL = 61;
            step   = 5;
            CStep  = 6;
        }
    }
    else if(down){   /*opponents start*/
        if(edge_down){
            edge_down=0;
            resetState();
            flagf = 1;
            targ_velocity = speed;
            sendData(7,70);
            stateR = 70;
            stateL = 70;
            step   = 15;
            CStep  = 15;
        }
    }
    else if(square){ /*reset*/
        sendData(7,255);
    }
    else if(cross){  /*stop*/
        Motor_swing=0;
//        sita=PI/4.0,x=0.0,y=0.0;
        targ_velocity=0.0;
#ifdef BLUE
        targ_sita=PI/4.0;
#else   
        targ_sita=-PI/4.0;
#endif
        velocity_controller.reset();
        direction_controller.reset();
    }
    else if(l1){  /*L up*/
        if(edge_l1){
            edge_l1=0;
            if(stateL!=1) stateL--;
            sendData(4,stateL);
        }
    }
    else if(l2){  /*L down*/
        if(edge_l2){
            edge_l2=0;
            if(stateL!=MAX_VALUE) stateL++;
            sendData(4,stateL);
        }
    }
    else if(r1){  /*R up*/
        if(edge_r1){
            edge_r1=0;
            if(stateR!=1) stateR--;
            sendData(5,stateR);
        }
    }
    else if(r2){  /*R down*/
        if(edge_r2){
            edge_r2=0;
            if(stateR!=MAX_VALUE) stateR++;
            sendData(5,stateR);
        }
    }
    if(triangle){
        skip = 1;
    }
    else if(!triangle){
        skip = 0;
    }
}
#endif
/***The function is PID controller initialize.***/
inline void initializeControllers()
{
//    velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
    velocity_controller.setInputLimits(-20000.0, 20000.0); //x1
    direction_controller.setInputLimits(-10.0, 10.0); //x2

    //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
//    velocity_controller.setOutputLimits(-1.0, 1.0);
    velocity_controller.setOutputLimits(0.0, 1.0);
    direction_controller.setOutputLimits(-1.0, 1.0);

    //set bias. 初期値
    velocity_controller.setBias(0.0);
    direction_controller.setBias(0.0);

    //set mode.
    velocity_controller.setMode(AUTO_MODE);
    direction_controller.setMode(AUTO_MODE);
}

/***The function is following move speed.***/
inline void velocity_following()
{
    velocity_controller.setSetPoint((float)targ_velocity);
    velocity_controller.setProcessValue((float)velocity);
    x1 = (double)velocity_controller.compute();
}

inline void sita_following()
{
    direction_controller.setSetPoint((float)targ_sita);
    direction_controller.setProcessValue((float)sita);
    //direction_controller.setSetPoint(0.0);  /*目標値とのずれをなくす*/
    //direction_controller.setProcessValue(-y);
    x2 = (double)direction_controller.compute();
}

inline void move_following()
{
    velocity_following();
    sita_following();
    
    if(flagf==0){
        Vr                  = ( 2.0*(-x1) + x2 ) / 3.0;
        Vl                  = ( 2.0*(-x1) - x2 ) / 3.0;
    }
    else if(flagf==1){
        if(x2>0){
            Vr                  = x1;
            Vl                  = x1 - x2;
        }
        else{
            Vr                  = x1 + x2;
            Vl                  = x1;
        }
    }
    
    if( abs(Vr) < 0.2 ) Vr = 0.0;
    if( abs(Vl) < 0.2 ) Vl = 0.0;
    Move_r( ( float ) Vr );
    Move_l( ( float ) Vl );
}

#endif /*autoMode.h*/