File content as of revision 1:7b5469bf5994:

#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)
//        {
//            if(idle_state == true)
//            {
//                idle_state = false;
//            }
            
            
            
            
            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]);
            
                
                
                
//        }
//        else
//        {
//                if(menual_mode)
//                {
//                    Menual_Position_PID();
//                    for(int i=0; i<6; i++)
//                        motor_power(i,Speed_PID_OUTPUT[i]);
//                }
//                else
//                {
//                    if(idle_state == false)
//                    {
//                        Idle_Enc_Check();
//                        idle_state = true;
//                        pc.printf("idle state..\r\n");
//                    }                 
//                    for(int i = 0; i < 6; i++)
//                        motor_power(i, 0);
//                }
//        }
            
    }
}

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;
    } 
}