File content as of revision 0:e12eb40b9fef:

/*
void initial_position()
{
    cmd_roll     =  32768;
    cmd_pitch    =  32768;
    cmd_heave    =  32768;
    cmd_sway     =  32768;
    cmd_surge    =  32768;
    cmd_yaw      =  32768;

    double base_position[6]= {0,0,0,0,0,0};
    double motor_duty=200;

    base_position[0]=-(origin_degree1/(1.5*90))*1024*100;
    base_position[1]=-(origin_degree2/(1.5*90))*1024*100;
    base_position[2]=-(origin_degree3/(1.5*90))*1024*100;
    base_position[3]=(origin_degree4/(1.5*90))*1024*100;
    base_position[4]=-(origin_degree5/(1.5*90))*1024*100;
    base_position[5]=-(origin_degree6/(1.5*90))*1024*100;


    bool base_position_check[6]= {false,false,false,false,false,false};

    int b_cnt=0;
    for(int i=0; i<4001; i++) {
        encoder_read();

        b_cnt++;
        if(b_cnt>499) {
            b_cnt=0;
            pc.printf("Base_positioning : %d \r\n",i+1);

            for(int k=0; k<6; k++) {
                pc.printf("%8.0f", base_position[k]);
            }
            pc.printf("\r\n");
            for(int k=0; k<6; k++) {
                pc.printf("%8d", encoder_data[k]);
            }
            pc.printf("\r\n");
        }



        for(int u=0; u<3; u++) {
            if(encoder_data[u]<base_position[u]) {
                motor_power(u, 0);
                base_position_check[u]=true;
            } else if(encoder_data[u]<base_position[u]*0.9)
                motor_power(u, motor_duty*0.5);
            else if(encoder_data[u]<base_position[u]*0.8)
                motor_power(u, motor_duty*0.7);
            else if(encoder_data[u]<base_position[u]*0.7)
                motor_power(u, motor_duty*0.9);
            else
                motor_power(u, motor_duty);
        }

        for(int u=4; u<6; u++) {
            if(encoder_data[u]<base_position[u]) {
                motor_power(u, 0);
                base_position_check[u]=true;
            } else if(encoder_data[u]<base_position[u]*0.9)
                motor_power(u, -motor_duty*0.5);
            else if(encoder_data[u]<base_position[u]*0.8)
                motor_power(u, -motor_duty*0.7);
            else if(encoder_data[u]<base_position[u]*0.7)
                motor_power(u, -motor_duty*0.9);
            else
                motor_power(u, -motor_duty);
        }
        
        for(int u=3; u<4; u++) {
            if(encoder_data[u]>base_position[u]) {
                motor_power(u, 0);
                base_position_check[u]=true;
            } else if(encoder_data[u]>base_position[u]*0.9)
                motor_power(u, motor_duty*0.5);
            else if(encoder_data[u]>base_position[u]*0.8)
                motor_power(u, motor_duty*0.7);
            else if(encoder_data[u]>base_position[u]*0.7)
                motor_power(u, motor_duty*0.9);
            else
                motor_power(u, motor_duty);
        }

    }

    motor_power(0, 0);
    motor_power(1, 0);
    motor_power(2, 0);
    motor_power(3, 0);
    motor_power(4, 0);
    motor_power(5, 0);
}
*/

/*
        m_cnt++;
        if(m_cnt>3000)
        {
            m_cnt=0;
            pc.printf("     Speed_Pgain     Speed_Igain     position_Pgain     Position_input_filter     Speed_I_input_filter     offset\r\n");
            for(int i = 0; i < 6; i++)
            {
                pc.printf("%2d.    %6.3f          %6.3f            %6.3f                %6.3f                    %6.3f             %4d\r\n", i + 1, Speed_Pgain[i], Speed_Igain[i], position_Pgain[i], Position_input_filter[i], Speed_I_input_filter[i], (int)offset[i]);
            }
            pc.printf("\n\n\n");
        }
        */