Revision 4:bf278ddb8504, committed 2020-12-08

Comitter:

lsh3146

Date:

Tue Dec 08 01:25:06 2020 +0000

Parent:

2:14b52dee1c35

Commit message:

aaaaaqqqqq

Changed in this revision

diff -r 14b52dee1c35 -r bf278ddb8504 Position.h
--- a/Position.h	Tue Jul 28 01:42:16 2020 +0000
+++ b/Position.h	Tue Dec 08 01:25:06 2020 +0000
@@ -10,7 +10,7 @@
 double gear[6]={1.5,1.5,1.5,1.5,1.5,1.5};
 
 double boot_cnt=0;
-int boot_cnt_max=15000;
+int boot_cnt_max=5000; // 서스 중량에 따라 조절 defalut : 15000
 
 void taget_position_read()
 {
@@ -25,10 +25,10 @@
     
     for(int i = 0; i < 3; i++)
     {
-        if(taget_position[i] > 160)
-        taget_position[i] = 160;
-        if(taget_position[i] < 0)
-        taget_position[i] = 0;
+        if(taget_position[i] > 100)
+        taget_position[i] = 100;
+        if(taget_position[i] < 20)
+        taget_position[i] = 20;
     }
     
 
@@ -40,10 +40,10 @@
     
     for(int i = 4; i < 6; i++)
     {
-        if(taget_position[i] > 110)
-        taget_position[i] = 110;
-        if(taget_position[i] < 5)
-        taget_position[i] = 5;
+        if(taget_position[i] > 100)
+        taget_position[i] = 100;
+        if(taget_position[i] < 10)
+        taget_position[i] = 10;
     }
 
 }

diff -r 14b52dee1c35 -r bf278ddb8504 encoder.h
--- a/encoder.h	Tue Jul 28 01:42:16 2020 +0000
+++ b/encoder.h	Tue Dec 08 01:25:06 2020 +0000
@@ -55,37 +55,16 @@
 */
 void reset_check()
 {
-    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)
-    {
-        motor_onoff[0]=false;
-        motor_onoff[1]=false;
-        motor_onoff[2]=false;
-        motor_onoff[3]=false;
-        motor_onoff[4]=false;
-        motor_onoff[5]=false;
+}
 
-        while(1)
-        {
-            
-        pwm1.write(0.4);
-        pwm2.write(0.4);
-        pwm3.write(0.4);
-        pwm4.write(0.4);
-        pwm5.write(0.4);
-        pwm6.write(0.4);
-        
-        wait_ms(50);
-        
+/*
             encoder_data[0] = encoder1.read();
             if(encoder_data[0]!=-1)
             {
                 NVIC_SystemReset();
             }
-                wait_ms(200);
-        }
+*/
         
-    } 
-}
 
 void encoder_reset_cnt()
 {

diff -r 14b52dee1c35 -r bf278ddb8504 idle_check.h
--- a/idle_check.h	Tue Jul 28 01:42:16 2020 +0000
+++ b/idle_check.h	Tue Dec 08 01:25:06 2020 +0000
@@ -1,68 +1,51 @@
 #ifndef _IDLE_CHECK_H_
 #define _IDLE_CHECK_H_
 
-#include "encoder.h"
-
-
-#define IDLE_CHECK_CNT  400 //loop(2.5ms) * IDLE_CHECK_CNT = idle check time
-
-
-uint8_t idle_check_var[100] = {0, };
-int idle_enc_data[6] = {0, };
-
-uint32_t idle_cnt = 0;
-bool idle_state = false;
+#define IDLE_CHECK_CNT  1400 //loop(2.5ms) * IDLE_CHECK_CNT = idle check time
 
-void Idle_Check_Init();
-bool Idle_Check();
-
+unsigned int ex_cmd_roll=0;
+unsigned int ex_cmd_pitch=0; 
+unsigned int ex_cmd_heave=0; 
+unsigned int ex_cmd_sway=0; 
+unsigned int ex_cmd_surge=0; 
+unsigned int ex_cmd_yaw=0;
 
-void Idle_Check_Init()
-{
-    for(int i = 0; i < 100; i++)
-    {
-        idle_check_var[i] = command[i];
-    }
-}
+bool idle_state = false;
+int idle_cnt = 0;
 
-void Idle_Enc_Check()
+void idle_check(unsigned int roll, unsigned int pitch, unsigned int heave, unsigned int sway, unsigned int surge, unsigned int yaw)
 {
-    idle_enc_data[0] = encoder_data[0];
-    idle_enc_data[1] = encoder_data[1];
-    idle_enc_data[2] = encoder_data[2];
-    idle_enc_data[3] = encoder_data[3];
-    idle_enc_data[4] = encoder_data[4];
-    idle_enc_data[5] = encoder_data[5];
-}
-
-bool Idle_Check()
-{
-    bool same_command_data = true;
     
-    for(int i = 0; i < 100; i++)
-    {
-        if(idle_check_var[i] != command[i])
+    if( roll    ==  ex_cmd_roll     &&
+        pitch   ==  ex_cmd_pitch    &&
+        heave   ==  ex_cmd_heave    &&
+        sway    ==  ex_cmd_sway     &&
+        surge   ==  ex_cmd_surge    &&
+        yaw     ==  ex_cmd_yaw )
+        {
+            idle_cnt++;
+        }else
         {
-            idle_check_var[i] = command[i];
-            same_command_data = false;
+            idle_cnt = idle_cnt - 50;
         }
-    }
-    
-    if(same_command_data == false)
-    {
-        idle_cnt = 0;
-    }
-    else
-    {
-        idle_cnt++;
-    }
-    
-    if(idle_cnt >= IDLE_CHECK_CNT)
-    {
-        return true;
-    }
-    
-    return false;
+        
+        if(idle_cnt > IDLE_CHECK_CNT)
+            idle_cnt = IDLE_CHECK_CNT;
+        if(idle_cnt < 0)
+            idle_cnt = 0;
+        
+        if(idle_cnt > 1100)
+            idle_state = true;
+        else
+            idle_state = false;
+        
+        
+        ex_cmd_roll = roll;
+        ex_cmd_pitch = pitch;
+        ex_cmd_heave = heave;
+        ex_cmd_sway = sway;
+        ex_cmd_surge = surge;
+        ex_cmd_yaw = yaw;
 }
 
 #endif
\ No newline at end of file

diff -r 14b52dee1c35 -r bf278ddb8504 limit.h
--- a/limit.h	Tue Jul 28 01:42:16 2020 +0000
+++ b/limit.h	Tue Dec 08 01:25:06 2020 +0000
@@ -3,6 +3,7 @@
 
 #define limit_debug_print false
 #define start_duty 200
+#define limit_max_power 500
 
 bool limit_find[6]={false,false,false,false,false,false};
 
@@ -68,9 +69,9 @@
     if(limit_count1<-50)
         limit_count1=-50;
         
-    if(limit_count1>30)
+    if(limit_count1>15)
         limit_result1=0;
-    if(limit_count1<-30)
+    if(limit_count1<-15)
         limit_count1=1;
 }
 
@@ -87,9 +88,9 @@
     if(limit_count2<-50)
         limit_count2=-50;
         
-    if(limit_count2>30)
+    if(limit_count2>15)
         limit_result2=0;
-    if(limit_count2<-30)
+    if(limit_count2<-15)
         limit_count2=1;
 }
 
@@ -106,9 +107,9 @@
     if(limit_count3<-50)
         limit_count3=-50;
         
-    if(limit_count3>30)
+    if(limit_count3>15)
         limit_result3=0;
-    if(limit_count3<-30)
+    if(limit_count3<-15)
         limit_count3=1;
 }
 
@@ -126,9 +127,9 @@
     if(limit_count4<-50)
         limit_count4=-50;
         
-    if(limit_count4>30)
+    if(limit_count4>15)
         limit_result4=0;
-    if(limit_count4<-30)
+    if(limit_count4<-15)
         limit_count4=1;
 }
 
@@ -146,9 +147,9 @@
     if(limit_count5<-50)
         limit_count5=-50;
         
-    if(limit_count5>30)
+    if(limit_count5>15)
         limit_result5=0;
-    if(limit_count5<-30)
+    if(limit_count5<-15)
         limit_count5=1;
 }
 
@@ -166,9 +167,9 @@
     if(limit_count6<-50)
         limit_count6=-50;
         
-    if(limit_count6>30)
+    if(limit_count6>15)
         limit_result6=0;
-    if(limit_count6<-30)
+    if(limit_count6<-15)
         limit_count6=1;
 }
 
@@ -321,10 +322,10 @@
         sum_error[i]+=limit_error[i]*0.2;
         limit_result[i]=limit_error[i]*2+sum_error[i]*1;
         
-        if(limit_result[i]>200)
-            limit_result[i]=200;
-        if(limit_result[i]<-200)
-            limit_result[i]=-200;
+        if(limit_result[i]>limit_max_power)
+            limit_result[i]=limit_max_power;
+        if(limit_result[i]<-limit_max_power)
+            limit_result[i]=-limit_max_power;
         
         limit_output[i]=motor_duty[i]+limit_result[i];
         
@@ -342,10 +343,10 @@
         sum_error[i]+=limit_error[i]*0.2;
         limit_result[i]=limit_error[i]*2+sum_error[i]*1;
         
-        if(limit_result[i]>200)
-            limit_result[i]=200;
-        if(limit_result[i]<-200)
-            limit_result[i]=-200;
+        if(limit_result[i]>limit_max_power)
+            limit_result[i]=limit_max_power;
+        if(limit_result[i]<-limit_max_power)
+            limit_result[i]=-limit_max_power;
         
         limit_output[i]=motor_duty[i]+limit_result[i];
 
@@ -363,10 +364,10 @@
         sum_error[i]+=limit_error[i]*0.2;
         limit_result[i]=limit_error[i]*2+sum_error[i]*1;
         
-        if(limit_result[i]>200)
-            limit_result[i]=200;
-        if(limit_result[i]<-200)
-            limit_result[i]=-200;
+        if(limit_result[i]>limit_max_power)
+            limit_result[i]=limit_max_power;
+        if(limit_result[i]<-limit_max_power)
+            limit_result[i]=-limit_max_power;
         
         limit_output[i]=motor_duty[i]+limit_result[i];
         
@@ -425,8 +426,11 @@
 
 bool limit_check_done=false;
 int limit_check_done_cnt=0;
+
 void find_limit()
 {
+    bool limit_sucess=false;
+    
     wait_ms(0);
     
     encoder_reset_cnt();
@@ -435,9 +439,31 @@
     {
         if(limit_check())
         {
+            limit_sucess=true;
             break;
         }
-        reset_check();
+        
+        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)
+                {
+                    motor_power(0,0);
+                    motor_power(1,0);
+                    motor_power(2,0);
+                    motor_power(3,0);
+                    motor_power(4,0);
+                    motor_power(5,0);
+    
+                    pc.printf("encoder_data[0]==-1 && encoder_data[1]==-1 && encoder_data[2]==-1 && encoder_data[3]==-1 && encoder_data[4]==1 && encoder_data[5]==1\r\n");
+                    //encoder_data[0] = encoder1.read();
+                    //if(encoder_data[0]!=-1)
+                    //{
+                        //NVIC_SystemReset();
+                    //}
+                    wait_us(2500);
+                }
+            }
+            
         wait_ms(1);
         
     }
@@ -448,6 +474,12 @@
     motor_power(3,0);
     motor_power(4,0);
     motor_power(5,0);
+    
+    while(!limit_sucess)
+    {
+        pc.printf("Limit-faild\r\n");
+    }
+    
 }
 
 #endif

diff -r 14b52dee1c35 -r bf278ddb8504 main.cpp
--- a/main.cpp	Tue Jul 28 01:42:16 2020 +0000
+++ b/main.cpp	Tue Dec 08 01:25:06 2020 +0000
@@ -1,6 +1,10 @@
 #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();
@@ -13,6 +17,63 @@
 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()
 {
     
@@ -32,53 +93,82 @@
     
     pc.printf("PID LOOP_START\r\n");
     pid_info();
-            
+    
+    int a_cnt=0;
 
     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();
 
+       
+            Position_PID();
+            Speed_PID();
 
-       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();
-    
+        int encoder_error[6] = {-1,-1,-1,-1,1,1};
             for(int i=0; i<6; i++)
-                motor_power(i,Speed_PID_OUTPUT[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;

diff -r 14b52dee1c35 -r bf278ddb8504 main.h
--- a/main.h	Tue Jul 28 01:42:16 2020 +0000
+++ b/main.h	Tue Dec 08 01:25:06 2020 +0000
@@ -5,6 +5,8 @@
 bool motor_onoff[6]={true,true,true,true,true,true};
 int encoder_data[6]={0,0,0,0,0,0};
 
+double motor_gain[6]={1,1,1,1,1,1};
+
 double Speed_Pgain[6]={ 3,
                         3,
                         3,
@@ -80,6 +82,7 @@
 #define MOTOR_PWM6      PA_10
 #define MOTOR_PWM7      PB_11
 
+#include "idle_check.h"
 #include "flash.h"
 #include "spi_setup.h"
 #include "pc_serial.h"
@@ -90,7 +93,6 @@
 #include "Position.h"
 #include "limit.h"
 #include "button.h"
-#include "idle_check.h"
 
 
 void board_init()
@@ -125,7 +127,6 @@
     spi_init();
     motor_init();
     Serial_Init();
-    Idle_Check_Init();
     
 }
 

diff -r 14b52dee1c35 -r bf278ddb8504 motor.h
--- a/motor.h	Tue Jul 28 01:42:16 2020 +0000
+++ b/motor.h	Tue Dec 08 01:25:06 2020 +0000
@@ -43,127 +43,55 @@
 
 int error_check_boost_duty[6]={0,};
 
+
+double motor_offset[6]={0,0,0,0,0,0};
+
+double last_percent[6]={0,};
+
 void motor_power(int motor_num,double percent)
 {
     
     percent=-percent;
     double output=offset[motor_num];
-    if(percent<-500)
-        percent=-500;
-    if(percent>500)
-        percent=500;
-    
-        
-    if(percent>150 || percent<-150)
-    {
-        now_encoder[motor_num]=encoder_data[motor_num];
-    
-        if(ex_encoder[motor_num]==now_encoder[motor_num])
-            stop_cnt[motor_num]++;
-        else
-            stop_cnt[motor_num]=0;
-    
-        ex_encoder[motor_num]=now_encoder[motor_num];
+    if(percent<-500*motor_gain[motor_num])
+        percent=-500*motor_gain[motor_num];
+    if(percent>500*motor_gain[motor_num])
+        percent=500*motor_gain[motor_num];
         
-        if(stop_cnt[motor_num]>300)
-        {
-            if(abs(error_check_boost_duty[motor_num])>500)
-                motor_onoff[motor_num]=false;  
-            else
-            {
-                if(percent<0)
-                    error_check_boost_duty[motor_num]--;
-                else
-                    error_check_boost_duty[motor_num]++;
-                
-                percent=percent+error_check_boost_duty[motor_num];
-            }
-                
-        }
-    }else
-    {
-      stop_cnt[motor_num]=0;  
-      
-      if(error_check_boost_duty[motor_num]<0)
-        error_check_boost_duty[motor_num]++;
-      else if(error_check_boost_duty[motor_num]>0)
-        error_check_boost_duty[motor_num]--;
         
-    }
-    
-    
-    
-        now_motor_duty[motor_num]=abs(percent);
-    
-    sum_duty_123axis    =   now_motor_duty[0]+now_motor_duty[1]+now_motor_duty[2]+now_motor_duty[3];
-    sum_duty_456axis    =   now_motor_duty[4]+now_motor_duty[5];
-    
-    total_duty=sum_duty_123axis;
-    
-    
-    if(total_duty>duty_limit)
-        motor_currnt_cut=(duty_limit/total_duty);
-    else
-        motor_currnt_cut=1.0;
-    
-    if(duty_limit_on)
-    {
-        if( (motor_num==1) || (motor_num==2) || (motor_num==3) || (motor_num==4) )
-            percent=percent*motor_currnt_cut;
-        
-        duty_limit_cnt++;
-    if(duty_limit_cnt>200)
-    {
-      duty_limit_cnt=0;
-      pc.printf("%0.1f,%4.0f,%4.0f,%4.0f,%4.0f\r\n",motor_currnt_cut,now_motor_duty[0],now_motor_duty[1],now_motor_duty[2],now_motor_duty[3]);  
-    }   
-    }
     
         if(motor_num==0)
-        {
-            if(motor_onoff[motor_num]==false)
-                percent=1000;
-                
+        {    
+            last_percent[motor_num]=percent;
             output = 1-(offset[motor_num] + percent)/2500;
             pwm1.write(output);
         }
         else if(motor_num==1)
         {
-            if(motor_onoff[motor_num]==false)
-                percent=1000;
-                
+            last_percent[motor_num]=percent;
             output = 1-(offset[motor_num] + percent)/2500;
             pwm2.write(output);
         }
         else if(motor_num==2)
         {
-            if(motor_onoff[motor_num]==false)
-                percent=1000;
-                
+            last_percent[motor_num]=percent;
             output = 1-(offset[motor_num] + percent)/2500;
             pwm3.write(output);
         }
         else if(motor_num==3)
-        {
-            if(motor_onoff[motor_num]==false)
-                percent=1000;
-                
+        {   last_percent[motor_num]=percent;        
             output = 1-(offset[motor_num] + percent)/2500;
             pwm4.write(output);
         }
         else if(motor_num==4)
         {
-            if(motor_onoff[motor_num]==false)
-                percent=1000;
-                
+            last_percent[motor_num]=percent;
             output = 1-(offset[motor_num] + percent)/2500;
             pwm5.write(output);
         }
         else if(motor_num==5)
         {
-            if(motor_onoff[motor_num]==false)
-                percent=1000;
-                
+            last_percent[motor_num]=percent;
             output = 1-(offset[motor_num] + percent)/2500;
             pwm6.write(output);
         }

diff -r 14b52dee1c35 -r bf278ddb8504 pc_serial.h
--- a/pc_serial.h	Tue Jul 28 01:42:16 2020 +0000
+++ b/pc_serial.h	Tue Dec 08 01:25:06 2020 +0000
@@ -9,7 +9,7 @@
 
 char check_pitch[6];
 char rx_buffer[100]={0,};
-char command[100]={0,};
+char command[100]="Start32768,32768,32768,32768,32768,32768\n";
 char tx_buffer[100]="txdat32768,32768,32768,32768,32768,32768\n";
 int tx_index = 0;
 int rx_index = 0;
@@ -21,7 +21,19 @@
 
 double set_data[100] = {0x00, }; 
 
-unsigned int cmd_roll, cmd_pitch, cmd_heave, cmd_sway, cmd_surge, cmd_yaw;
+unsigned int cmd_roll=32768;
+unsigned int cmd_pitch=32768;
+unsigned int cmd_heave=32768;
+unsigned int cmd_sway=32768;
+unsigned int cmd_surge=32768;
+unsigned int cmd_yaw=32768;
+
+unsigned int A_ex_cmd_roll=32768;
+unsigned int A_ex_cmd_pitch=32768;
+unsigned int A_ex_cmd_heave=32768;
+unsigned int A_ex_cmd_sway=32768;
+unsigned int A_ex_cmd_surge=32768;
+unsigned int A_ex_cmd_yaw=32768;
 
 void Start_Command();
 void Set_Arrangement();
@@ -104,7 +116,6 @@
                 temp=temp*10+command[i]-0x30;
         }
         
- 
         cmd_roll     =  data1;
         cmd_pitch    =  data2;
         cmd_heave    =  data3;
@@ -112,6 +123,9 @@
         cmd_surge    =  data5;
         cmd_yaw      =  data6;
         
+        idle_check(cmd_roll,cmd_pitch,cmd_heave,cmd_sway,cmd_surge,cmd_yaw);
+        
+        
         /*
         pc.printf("%d,",cmd_roll);
         pc.printf("%d,",cmd_pitch);

diff -r 14b52dee1c35 -r bf278ddb8504 speed_pid.h
--- a/speed_pid.h	Tue Jul 28 01:42:16 2020 +0000
+++ b/speed_pid.h	Tue Dec 08 01:25:06 2020 +0000
@@ -2,38 +2,67 @@
 #define _SPEED_PID_H_
 
 double taget_speed[6]={0,};
+double ex_taget_speed[6]={0,};
+double dif_taget_speed[6]={0,};
+double filterd_dif_taget_speed[6]={0,0,0,0,0,0,};
+
 double error_speed[6]={0,};
 double Speed_PID_OUTPUT[7]={0,};
+double ex_Speed_PID_OUTPUT[7]={0,};
 
 void cal_speed_error()
 {
     for(int i=0;i<6;i++)
     error_speed[i]=filter_dif_encoder_data[i]-taget_speed[i];
     
+    for(int i=0;i<6;i++)
+    {
+        dif_taget_speed[i] = ex_taget_speed[i] - taget_speed[i];
+        ex_taget_speed[i]=taget_speed[i];
+        
+        filterd_dif_taget_speed[i] = filterd_dif_taget_speed[i]*0.99 + dif_taget_speed[i]*0.01;
+    }
+    
 }
 
 double result_i[6]={0,};
 double filter_result_i[6]={0,};
 double output_i[6]={0,};
 
+
 void Speed_I()
 {
     for(int i=0;i<6;i++)
     {
-        if(filter_dif_encoder_data[i]<taget_speed[i])
-            result_i[i]+=1*Speed_Igain[i];
-        else if(filter_dif_encoder_data[i]>taget_speed[i])
-            result_i[i]-=1*Speed_Igain[i]; 
-            
-        filter_result_i[i] = filter_result_i[i]*(1-Speed_I_input_filter[i]) + result_i[i]*Speed_I_input_filter[i];
-        output_i[i] = - filter_result_i[i];
+        result_i[i]+=error_speed[i]*Speed_Igain[i];
+        output_i[i] = result_i[i];
+        
+        
+        if(output_i[i]>100)
+            output_i[i]=100;
+        if(output_i[i]<-100)
+            output_i[i]=-100;
     }
 }
 double output_p[6]={0,};
 void Speed_P()
 {
     for(int i=0;i<6;i++)
+    {
         output_p[i] =  error_speed[i] * Speed_Pgain[i];
+    }
+    
+    for(int i=0;i<3;i++)
+    {
+        int p_range = motor_gain[i]*500;
+        
+        if(output_p[i]>p_range)
+            output_p[i]=p_range;
+        if(output_p[i]<-p_range)
+            output_p[i]=-p_range;
+    }
+
+        
 }
 
 void Speed_PID()

diff -r 14b52dee1c35 -r bf278ddb8504 target_position_cal.h
--- a/target_position_cal.h	Tue Jul 28 01:42:16 2020 +0000
+++ b/target_position_cal.h	Tue Dec 08 01:25:06 2020 +0000
@@ -14,12 +14,12 @@
 #define pitch_gain 15 // pitch 게인 ( 부호를 바꾸면 방향이 바뀐다.)
 #define heave_gain 15 // heave_gain 게인 ( 부호를 바꾸면 방향이 바뀐다.)
 
-double origin_degree1=50;    // 모터1 기준위치 지정
-double origin_degree2=50;    // 모터2 기준위치 지정
-double origin_degree3=50;    // 모터3 기준위치 지정
+double origin_degree1=60;    // 모터1 기준위치 지정
+double origin_degree2=60;    // 모터2 기준위치 지정
+double origin_degree3=60;    // 모터3 기준위치 지정
 double origin_degree4=76;    // 모터2 기준위치 지정
-double origin_degree5=50;    // 모터3 기준위치 지정
-double origin_degree6=50;    // 모터3 기준위치 지정
+double origin_degree5=60;    // 모터3 기준위치 지정
+double origin_degree6=60;    // 모터3 기준위치 지정
 double cal_scale=0.7;  // 전체 게인 조절
 
 double cal_roll=0;