Diff: main.cpp

Revision:

1:7b5469bf5994

Parent:

0:7cff999a7f5c

Child:

2:14b52dee1c35

--- a/main.cpp	Mon May 11 08:47:18 2020 +0000
+++ b/main.cpp	Mon Jul 27 23:29:30 2020 +0000
@@ -5,12 +5,14 @@
 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()
 {
     
@@ -28,45 +30,74 @@
 
     command_init();
     
-    Button_Init();
-    
     pc.printf("PID LOOP_START\r\n");
     pid_info();
             
 
-    while(1) {
-        
+    while(1)
+    {      
         reset_check();
         
         m_cnt++;
         if(m_cnt>1000)
         {
             m_cnt=0;
-            //pid_info();
         }
 
         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(!menual_mode)
-            Position_PID();
-        else
-            Menual_Position_PID();   
-        
-        Speed_PID();
-
-        for(int i=0; i<6; i++)
-            motor_power(i,Speed_PID_OUTPUT[i]);
+//        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;