File content as of revision 26:8e6c736b6791:

#ifndef AUTOMODE_H
#define AUTOMODE_H

/***PID Controller***/
PID velocity_controller(18.0,5274.0 ,0.0,RATE);
PID direction_controller(9.0,2637.0,0.0,RATE);

/***Ps3 correspondence***/
void autoPs3(){
    /*if(up) targ_velocity=speed;
    else if(down) targ_velocity=-speed;
    else if(right) targ_velocity=0.0,targ_sita=0.0;
    else if(left) targ_velocity=0.0,targ_sita=PI/2.0;
    else if(r1) targ_sita=0.0;
    else if(l1) targ_sita=PI/2.0;
    else if(square) if(edge_square) edge_square=0,autoflag=0,Indicator4=0;
    else if(cross){
        sendData(1, 4);
        sita=PI/4.0,x=0.0,y=0.0;
        targ_velocity=0.0;
        targ_sita=PI/4.0;
        velocity_controller.reset();
        direction_controller.reset();
        wait(0.3);
        sita=PI/4.0,x=0.0,y=0.0;
        velocity_controller.reset();
        direction_controller.reset();
    }
    else if(r2){
        if(edge_r1){
            edge_r1=0;
            sendData(1, 1);
            wait(0.1);
            sendData(1, 4);
            wait(0.01);
            sendData(1, 2);
            wait(0.1);
            sendData(1, 4);
        }
    }
    else if(l2){
        if(edge_l1){
            edge_l1=0;
            sendData(1, 3);
            wait(0.1);
            sendData(1, 4);
            wait(0.01);
            sendData(1, 5);
            wait(0.1);
            sendData(1, 7);
        }
    }
    else if(triangle){
        if(edge_triangle){
            edge_triangle=0;
            if(cylinder_step==3) cylinder_step=0;
            cylinder_step++;
            sendData(1, cylinder_step);
        }
    }*/
}




/***The function is PID controller initialize.***/
inline void initializeControllers()
{
    velocity_controller.setInputLimits(-200000.0, 200000.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);
    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();
    Vr                  = ( x1 + x2 ) / 2.0;
    Vl                  = ( x1 - x2 ) / 2.0;
    if( abs(Vr) < 0.05 ) Vr = 0.0;
    if( abs(Vl) < 0.05 ) Vl = 0.0;
    Move_r( ( float ) Vr );
    Move_l( ( float ) Vl );
}

#endif /*autoMode.h*/