Diff: main.cpp

Revision:

0:c21936a3520a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,84 @@
+#include "mbed.h"
+#include "main.h"
+
+//DigitalOut myled(LED1);
+
+
+int main()
+{
+
+    board_init();
+    
+    //encoder_check(); 
+    find_limit();
+    
+    //pc.printf("Work~\n");
+    
+    timer_init();
+    while(1)
+    { 
+
+    
+    bool_cnt++;
+    if(bool_cnt>10000)
+    bool_cnt=10000;
+    /*
+        pc.printf("%5.3f,",first_degree);
+        pc.printf("%5.3f,",second_degree);
+        pc.printf("%5.3f,",third_degree);
+        pc.printf("%5.3f,",four_degree);
+        pc.printf("%5.3f,",five_degree);
+        pc.printf("%5.3f,",six_degree);
+        pc.printf("\n");
+        */
+        
+        
+        //loop_time_run();
+        //wait_us(2000);
+        
+        //Read_DATA();
+        
+        comunication(); // 모터의 위치 값을 받음
+        
+       // not_play_check();
+       
+       if(bool_cnt<2500)
+       {
+                cmd_roll     =  32768;
+                cmd_pitch    =  32768;
+                cmd_heave    =  32768;
+                cmd_sway     =  32768;
+                cmd_surge    =  32768;
+                cmd_yaw      =  32768;
+        }
+        
+        taget_position_cal((double)cmd_roll,(double)cmd_pitch,(double)cmd_heave,(double)cmd_sway,(double)cmd_surge,(double)cmd_yaw); 
+        
+        //pc.printf("%s",command);
+
+/*
+        pc.printf("%d,",cmd_roll);
+        pc.printf("%d,",cmd_pitch);
+        pc.printf("%d,",cmd_heave);
+        pc.printf("%d,",cmd_sway);
+        pc.printf("%d,",cmd_surge);
+        pc.printf("%d,",cmd_yaw);
+        pc.printf("\n");
+*/
+
+        
+        encoder_read();
+        
+        Position_PID();
+        
+        Speed_PID();
+        
+        for(int i=0;i<6;i++)
+        motor_power(i,Speed_PID_OUTPUT[i]);
+        //tor_power(i,200);
+        
+
+    }
+}
+
+