File content as of revision 2:14b52dee1c35:

#include "mbed.h"
#include "main.h"

void initial_position();
void begin_pid();
void command_init();
void pid_info();
void Menual_Position_PID();
void test();

int cnt=0;

int m_cnt=0;
int menual=false;

int main()
{
    
    board_init();
    MCP23S17_Init();
    Segment_Init();
    Button_Init();
    
    printf("READ Gain pgain : %ld, input : %ld\r\n", Button_Read_Flash(0), Button_Read_Flash(4));
    

    encoder_check2();

    find_limit();

    command_init();
    
    pc.printf("PID LOOP_START\r\n");
    pid_info();
            

    while(1)
    {      
        reset_check();
        
        m_cnt++;
        if(m_cnt>1000)
        {
            m_cnt=0;
        }

        Button_Detection();

        comunication(); // 모터의 위치 값을 받음
        taget_position_cal((double)cmd_roll,(double)cmd_pitch,(double)cmd_heave,(double)cmd_sway,(double)cmd_surge,(double)cmd_yaw);
        encoder_read();


       if(Idle_Check() == false)
       {
           for(int i=0;i<6;i++)
           {
            Speed_Igain[i]=0.01;
           }
       }
       else
       {
           for(int i=0;i<6;i++)
           {
            Speed_Igain[i]=0;
           }
        }
       
            if(!menual_mode)
                Position_PID();
            else
                Menual_Position_PID();   
             
            Speed_PID();
    
            for(int i=0; i<6; i++)
                motor_power(i,Speed_PID_OUTPUT[i]);
      
    }
}

void command_init()
{
    cmd_roll     =  32768;
    cmd_pitch    =  32768;
    cmd_heave    =  32768;
    cmd_sway     =  32768;
    cmd_surge    =  32768;
    cmd_yaw      =  32768;
}


void pid_info()
{
        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");
            
}
            
void Menual_Position_PID()
{
    for(int i=0; i<6; i++)
    {
        taget_speed[i]=button_offset_posion[i]/5;
    } 
}