File content as of revision 0:e12eb40b9fef:

#ifndef _SPEED_PID_H_
#define _SPEED_PID_H_

double taget_speed[6]={0,};
double error_speed[6]={0,};
double 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];
    
}

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++)
    {
        if(filter_dif_encoder_data[i]<taget_speed[i])
            result_i[i]+=1*Speed_Igain[i];
        else if(filter_dif_encoder_data[i]>taget_speed[i])
            result_i[i]-=1*Speed_Igain[i]; 
            
        filter_result_i[i] = filter_result_i[i]*(1-Speed_I_input_filter[i]) + result_i[i]*Speed_I_input_filter[i];
        output_i[i] = - filter_result_i[i];
    }
}
double output_p[6]={0,};
void Speed_P()
{
    for(int i=0;i<6;i++)
        output_p[i] =  error_speed[i] * Speed_Pgain[i];
}

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