Diff: main.cpp

Revision:

2:14b52dee1c35

Parent:

1:7b5469bf5994

Child:

3:7b195612e26d

Child:

4:bf278ddb8504

--- a/main.cpp	Mon Jul 27 23:29:30 2020 +0000
+++ b/main.cpp	Tue Jul 28 01:42:16 2020 +0000
@@ -50,16 +50,22 @@
         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(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
@@ -69,32 +75,7 @@
     
             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);
-//                }
-//        }
-            
+      
     }
 }