File content as of revision 4:bf278ddb8504:

#ifndef _SPEED_PID_H_
#define _SPEED_PID_H_

double taget_speed[6]={0,};
double ex_taget_speed[6]={0,};
double dif_taget_speed[6]={0,};
double filterd_dif_taget_speed[6]={0,0,0,0,0,0,};

double error_speed[6]={0,};
double Speed_PID_OUTPUT[7]={0,};
double ex_Speed_PID_OUTPUT[7]={0,};

void cal_speed_error()
{
    for(int i=0;i<6;i++)
    error_speed[i]=filter_dif_encoder_data[i]-taget_speed[i];
    
    for(int i=0;i<6;i++)
    {
        dif_taget_speed[i] = ex_taget_speed[i] - taget_speed[i];
        ex_taget_speed[i]=taget_speed[i];
        
        filterd_dif_taget_speed[i] = filterd_dif_taget_speed[i]*0.99 + dif_taget_speed[i]*0.01;
    }
    
}

double result_i[6]={0,};
double filter_result_i[6]={0,};
double output_i[6]={0,};


void Speed_I()
{
    for(int i=0;i<6;i++)
    {
        result_i[i]+=error_speed[i]*Speed_Igain[i];
        output_i[i] = result_i[i];
        
        
        if(output_i[i]>100)
            output_i[i]=100;
        if(output_i[i]<-100)
            output_i[i]=-100;
    }
}
double output_p[6]={0,};
void Speed_P()
{
    for(int i=0;i<6;i++)
    {
        output_p[i] =  error_speed[i] * Speed_Pgain[i];
    }
    
    for(int i=0;i<3;i++)
    {
        int p_range = motor_gain[i]*500;
        
        if(output_p[i]>p_range)
            output_p[i]=p_range;
        if(output_p[i]<-p_range)
            output_p[i]=-p_range;
    }

        
}

void Speed_PID()
{
    cal_speed_error();
    Speed_I();
    Speed_P();
    
    for(int i=0;i<6;i++)
    {
        Speed_PID_OUTPUT[i] = output_i[i] + output_p[i];
        if(i > 2)
        {
            Speed_PID_OUTPUT[i] *= -1;
        }
    }
    
    //pc.printf("%5.2f,%5.2f,%5.2f,%5.2f,%5.2f,%5.2f",error_speed[0],filter_dif_encoder_data[0],taget_speed[0],Speed_PID_OUTPUT[0],output_i[0],output_p[0]);
    //pc.printf("\n");
    
}

#endif