Diff: main.cpp

Revision:

0:7cff999a7f5c

Child:

1:7b5469bf5994

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 11 08:47:18 2020 +0000
@@ -0,0 +1,100 @@
+#include "mbed.h"
+#include "main.h"
+
+void initial_position();
+void begin_pid();
+void command_init();
+void pid_info();
+void test();
+
+int cnt=0;
+
+int m_cnt=0;
+int menual=false;
+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();
+    
+    Button_Init();
+    
+    pc.printf("PID LOOP_START\r\n");
+    pid_info();
+            
+
+    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]);
+            
+    }
+}
+
+
+
+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;
+    } 
+}
+
+