File content as of revision 4:bf278ddb8504:

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

#define sleep_duty 7

int Analysis_limit=2000;// change motor sleep

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;

double speed_sleep[6]={0,0,0,0,0,0};


int Analysis_data[400]={0};
int Analysis_cnt=0;
int Analysis_total=0;

int Analysis_sleep_cnt=0;

int stay_cnt=0;

double speed_0p5sec[6]={0,};

void input_data_Analysis()
{
    for(int i=0;i<6;i++)
    {
        speed_0p5sec[i]=speed_0p5sec[i]*0.99 + dif_encoder_data[i]*0.01;
    }
       
                if(boot_cnt>10000)
                {
                    for(int i=0;i<6;i++)
                    {
                    
                    if(filterd_dif_taget_speed[i]<0.01 && filterd_dif_taget_speed[i]>-0.01)
                    {
                        if(speed_0p5sec[i]<0.1 && speed_0p5sec[i]>-0.1)
                        {
                            speed_sleep[i]++;
                            if(speed_sleep[i]>(100-sleep_duty))
                            speed_sleep[i]=(100-sleep_duty);

                            motor_gain[i]=(100-speed_sleep[i])/100;
                        }
                    }else if(filterd_dif_taget_speed[i]>0.01 || filterd_dif_taget_speed[i]<-0.01)
                    {
                            speed_sleep[i]--;
                            if(speed_sleep[i]<0)
                            speed_sleep[i]=0;
                        motor_gain[i]=(100-speed_sleep[i])/100;
                    }
                    
                    }
                
                }
          
    //pc.printf("(%0.3f,%0.3f,%0.3f)",filterd_dif_taget_speed[0],filterd_dif_taget_speed[1],filterd_dif_taget_speed[2]);
    pc.printf("speed_sleep ( %d, %d, %d  %d, %d, %d )",(int)(speed_sleep[0]),(int)(speed_sleep[1]),(int)(speed_sleep[2]),(int)(speed_sleep[3]),(int)(speed_sleep[4]),(int)(speed_sleep[5]));
    pc.printf("gain ( %d, %d, %d  %d, %d, %d )",(int)(motor_gain[0]*100),(int)(motor_gain[1]*100),(int)(motor_gain[2]*100),(int)(motor_gain[3]*100),(int)(motor_gain[4]*100),(int)(motor_gain[5]*100));
    pc.printf("0.2s (%0.3f,%0.3f,%0.3f)",speed_0p5sec[0],speed_0p5sec[1],speed_0p5sec[2]);
    pc.printf("duty(%d,%d,%d,%d,%d,%d)\r\n",(int)last_percent[0],(int)last_percent[1],(int)last_percent[2],(int)last_percent[3],(int)last_percent[4],(int)last_percent[5]);
 
    
 }


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();
    
    int a_cnt=0;

    while(1)
    { 
    
        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();

       
            Position_PID();
            Speed_PID();

        int encoder_error[6] = {-1,-1,-1,-1,1,1};
            for(int i=0; i<6; i++)
            {
                if(encoder_data[i]==encoder_error[i])
                    motor_power(i,motor_offset[i]);
                else
                    motor_power(i,Speed_PID_OUTPUT[i]);
            }
            
            if(filter_dif_encoder_data[4]<=2 && filter_dif_encoder_data[4]>=-2)
            {
                if(Speed_PID_OUTPUT[4]>motor_offset[4])
                        motor_offset[4]+=0.2;
                else if(Speed_PID_OUTPUT[4]<motor_offset[4])
                        motor_offset[4]-=0.2;
            }
            
            if(filter_dif_encoder_data[5]<=2 && filter_dif_encoder_data[5]>=-2)
            {
                if(Speed_PID_OUTPUT[5]>motor_offset[5])
                        motor_offset[5]+=0.2;
                else if(Speed_PID_OUTPUT[5]<motor_offset[5])
                        motor_offset[5]-=0.2;
            }
            
            //pc.printf("%d,%d,%d,%d,%d,%d\r\n",(int)filter_dif_encoder_data[4],(int)filter_dif_encoder_data[5],(int)motor_offset[4],(int)motor_offset[5],(int)Speed_PID_OUTPUT[4],(int)Speed_PID_OUTPUT[5]);
            
            if(encoder_data[0]==-1 && encoder_data[1]==-1 && encoder_data[2]==-1 && encoder_data[3]==-1 && encoder_data[4]==1 && encoder_data[5]==1)
            {
                while(1)
                {
                    pc.printf("while(1)\r\n");
                    pc.printf("%d,%d,%d,%d\r\n",(int)motor_offset[4],(int)motor_offset[5],(int)Speed_PID_OUTPUT[4],(int)Speed_PID_OUTPUT[5]);

                    encoder_data[0] = encoder1.read();
                    if(encoder_data[0]!=-1)
                    {
                        NVIC_SystemReset();
                    }
                    wait_us(2500);
                }
            }
            
    a_cnt++;
    if(a_cnt>9)
    {
        a_cnt=0;
        input_data_Analysis();
    }
      
      A_ex_cmd_roll=cmd_roll;
      A_ex_cmd_pitch=cmd_pitch;
      A_ex_cmd_heave=cmd_heave;
      A_ex_cmd_sway=cmd_sway;
      A_ex_cmd_surge=cmd_surge;
      A_ex_cmd_yaw=cmd_yaw;
    }
}


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