File content as of revision 4:bf278ddb8504:

#ifndef _POSITION_H_
#define _POSITION_H_

double taget_position[6]={0,};
double oder_position[6]={0,};

double now_degree[6]={0,};
double error_degree[6]={0,};

double gear[6]={1.5,1.5,1.5,1.5,1.5,1.5};

double boot_cnt=0;
int boot_cnt_max=5000; // 서스 중량에 따라 조절 defalut : 15000

void taget_position_read()
{

    taget_position[0]=first_degree;
    taget_position[1]=second_degree;
    taget_position[2]=third_degree;
    taget_position[3]=-four_degree;
    taget_position[4]=five_degree;
    taget_position[5]=six_degree;
    
    
    for(int i = 0; i < 3; i++)
    {
        if(taget_position[i] > 100)
        taget_position[i] = 100;
        if(taget_position[i] < 20)
        taget_position[i] = 20;
    }
    

        if(taget_position[3] > 0)
        taget_position[3] = 0;
        if(taget_position[3] < -152)
        taget_position[3] = -152;

    
    for(int i = 4; i < 6; i++)
    {
        if(taget_position[i] > 100)
        taget_position[i] = 100;
        if(taget_position[i] < 10)
        taget_position[i] = 10;
    }

}

void input_filter()
{
    
    
for(int i=0;i<6;i++)
    oder_position[i] = oder_position[i]*(1-Position_input_filter[i]*(boot_cnt/boot_cnt_max)) + taget_position[i]*Position_input_filter[i]*(boot_cnt/boot_cnt_max);
}

void cal_error()
{
    for(int i=0;i<6;i++)
    error_degree[i] = oder_position[i]-now_degree[i];
}

void cal_degree()
{
  for(int i=0;i<6;i++)
  now_degree[i] = -(((double)encoder_data[i]/100)/1024)*90 * gear[i];
}

void Position_P()
{
        for(int i=0;i<6;i++)
            taget_speed[i]=-error_degree[i]*position_Pgain[i];

}



void Position_PID()
{
  boot_cnt++;
    if(boot_cnt>boot_cnt_max)
    boot_cnt=boot_cnt_max;
    
  taget_position_read();

  input_filter();
        
  cal_degree();
  
  cal_error();
  
  Position_P();

}

#endif