Diff: main.cpp

Revision:

4:bf278ddb8504

Parent:

2:14b52dee1c35

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