Revision 0:c21936a3520a, committed 2020-03-23

Comitter:

lsh2205

Date:

Mon Mar 23 08:38:40 2020 +0000

Commit message:

ss

Changed in this revision

diff -r 000000000000 -r c21936a3520a BufferedSerial.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BufferedSerial.lib	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/sam_grove/code/BufferedSerial/#7e5e866edd3d

diff -r 000000000000 -r c21936a3520a FastPWM.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastPWM.lib	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Sissors/code/FastPWM/#d6c2b73d71f5

diff -r 000000000000 -r c21936a3520a LS7366LIB.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LS7366LIB.lib	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/lsh2205/code/LS7366LIB/#e4cec33fe9eb

diff -r 000000000000 -r c21936a3520a Position.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Position.h	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,61 @@
+double taget_position[6]={0,};
+double oder_position[6]={0,};
+
+double now_degree[6]={0,};
+double error_degree[6]={0,};
+
+double gear[6]={1.5,1.5,1.5,1.5,1.5,1.5};
+
+void taget_position_read()
+{
+    
+    taget_position[0]=first_degree;
+    taget_position[1]=second_degree;
+    taget_position[2]=third_degree;
+    taget_position[3]=four_degree;
+    taget_position[4]=five_degree;
+    taget_position[5]=six_degree;
+    
+
+}
+
+void input_filter()
+{
+for(int i=0;i<6;i++)
+    oder_position[i] = oder_position[i]*(1-Position_input_filter[i]) + taget_position[i]*Position_input_filter[i];
+}
+
+void cal_error()
+{
+    for(int i=0;i<6;i++)
+    error_degree[i] = oder_position[i]-now_degree[i];
+}
+
+void cal_degree()
+{
+  for(int i=0;i<6;i++)
+  now_degree[i] = -(((double)encoder_data[i]/100)/1024)*90 * gear[i];
+}
+
+void Position_P()
+{
+    for(int i=0;i<6;i++)
+        taget_speed[i]=error_degree[i]*position_Pgain[i];
+}
+
+
+
+void Position_PID()
+{
+  taget_position_read();
+
+  input_filter();
+        
+  cal_degree();
+  
+  cal_error();
+  
+  Position_P();
+
+}
+

diff -r 000000000000 -r c21936a3520a SPI_TO_UART.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SPI_TO_UART.h	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,17 @@
+DigitalOut cs(PB_4);
+
+void Read_DATA()
+{
+  cs = 1;
+  for(int i=0; i<64; i++)
+  {
+    char c = spi1.write(0);
+    if(c==255)
+    {
+    break;
+    }
+    else
+    pc.printf("%c",c);
+  }   
+  cs = 0;
+}
\ No newline at end of file

diff -r 000000000000 -r c21936a3520a encoder.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoder.h	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,35 @@
+#ifndef _ENCODER_H_
+#define _ENCODER_H_
+
+int encoder_data[6]={0,};
+
+int ex_encoder_data[6]={0,};
+int dif_encoder_data[6]={0,};
+double filter_dif_encoder_data[6]={0,};
+double filter_dif_encoder_co[6]     = {0.01,0.01,0.01,0.01,0.01,0.01};
+
+void filter_encoder_data()
+{
+        for(int i=0; i<6;i++)
+        {
+        dif_encoder_data[i]= encoder_data[i] - ex_encoder_data[i];
+        
+        filter_dif_encoder_data[i] = filter_dif_encoder_data[i]*(1-filter_dif_encoder_co[i]) + (double)dif_encoder_data[i]*filter_dif_encoder_co[i];
+        
+        ex_encoder_data[i] = encoder_data[i];
+        }
+}
+
+void encoder_read()
+{
+        encoder_data[0] = encoder1.read();
+        encoder_data[1] = encoder2.read();
+        encoder_data[2] = encoder3.read();
+        encoder_data[3] = encoder4.read();
+        encoder_data[4] = encoder5.read();
+        encoder_data[5] = encoder6.read();
+        
+        filter_encoder_data();
+}
+
+#endif

diff -r 000000000000 -r c21936a3520a limit.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/limit.h	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,96 @@
+DigitalIn limit_sw1(LIMIT_SW1);
+DigitalIn limit_sw2(LIMIT_SW2);
+DigitalIn limit_sw3(LIMIT_SW3);
+DigitalIn limit_sw4(LIMIT_SW4);
+DigitalIn limit_sw5(LIMIT_SW5);
+DigitalIn limit_sw6(LIMIT_SW6);
+
+#define limit_time 10000
+
+bool limit_find[6]={false,false,false,false,false,false};
+
+void limit_init()
+{
+    limit_sw1.mode(PullUp);
+    limit_sw2.mode(PullUp);
+    limit_sw3.mode(PullUp);
+    limit_sw4.mode(PullUp);
+    limit_sw5.mode(PullUp);
+    limit_sw6.mode(PullUp);
+}
+
+bool limit_check123()
+{
+    limit_sw1.mode(PullUp);
+    limit_sw2.mode(PullUp);
+    limit_sw3.mode(PullUp);
+    
+    int sw1 = limit_sw1;
+    int sw2 = limit_sw2;
+    int sw3 = limit_sw3;
+
+    //pc.printf("Limit state : %d , %d , %d ",sw1,sw2,sw3);
+    //pc.printf("\n");
+    
+    
+    if(limit_find[0]==false)
+        motor_power(0,-150);
+    if(limit_find[1]==false)
+        motor_power(1,-150);
+    if(limit_find[2]==false)
+        motor_power(2,-150);
+    
+    if(sw1==0 && !limit_find[0])
+    {
+    limit_find[0]=true;
+    //pc.printf("find_limit : axis [ 1 ] ");
+    //pc.printf("\n");
+    motor_power(0,0);
+    encoder1.LS7366_reset_counter();
+    }
+    
+    if(sw2==0 && !limit_find[1])
+    {
+    limit_find[1]=true;
+    //pc.printf("find_limit : axis [ 2 ] ");
+    //pc.printf("\n");
+    motor_power(1,0);
+    encoder2.LS7366_reset_counter();
+    }
+
+    if(sw3==0 && !limit_find[2])
+    {
+    limit_find[2]=true;
+    //pc.printf("find_limit : axis [ 3 ] ");
+    //pc.printf("\n");
+    motor_power(2,0);
+    encoder3.LS7366_reset_counter();
+    }
+
+    
+    
+    return limit_find[0] && limit_find[1] && limit_find[2];
+}
+
+void find_limit()
+{
+    wait_ms(100);
+    //pc.printf("find_limit_start~~");
+    //pc.printf("\n");
+    for(int i=0;i<limit_time;i++)
+    {
+        if(limit_check123())
+        {
+            break;
+        }
+        wait_ms(1);
+        /*
+        if(i==limit_time)
+        {
+            pc.printf("Can not find LIMIT !!!");
+            pc.printf("\n");
+        }
+        */
+    }   
+        
+}
\ No newline at end of file

diff -r 000000000000 -r c21936a3520a main.cpp
--- /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);
+        
+
+    }
+}
+
+

diff -r 000000000000 -r c21936a3520a main.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,104 @@
+double Speed_Igain[6]={ 0.1,
+                        0.1,
+                        0.1,
+                        0.1,
+                        0.1,
+                        0.1};
+double Speed_Pgain[6]={ 5,
+                        5,
+                        5,
+                        5,
+                        5,
+                        5};
+double Position_input_filter[6]
+                     ={ 0.05,
+                        0.05,
+                        0.05,
+                        0.05,
+                        0.05,
+                        0.05};
+
+double position_Pgain[6]
+                     ={ 20,
+                        20,
+                        20,
+                        20,
+                        20,
+                        20};
+
+double offset[6]={      1480,
+                        1480,
+                        1480,
+                        1480,
+                        1480,
+                        1480};
+                        
+double Speed_I_input_filter[6]
+                     ={ 0.005,
+                        0.005,
+                        0.005,
+                        0.005,
+                        0.005,
+                        0.005};
+
+
+// SPI
+#define SPI_MOSI1       PA_7
+#define SPI_MISO1       PA_6
+#define SPI_SCK1        PA_5
+
+// Encoder
+#define ENCODER_CS1     PB_0
+#define ENCODER_CS2     PC_1
+#define ENCODER_CS3     PC_0
+#define ENCODER_CS4     PB_15
+#define ENCODER_CS5     PB_14
+#define ENCODER_CS6     PB_13
+
+// Limit switch
+
+#define LIMIT_SW1       PC_3
+#define LIMIT_SW2       PC_2
+#define LIMIT_SW3       PC_5
+#define LIMIT_SW4       PC_9
+#define LIMIT_SW5       PC_6
+#define LIMIT_SW6       PC_8
+
+// Motor
+#define MOTOR_PWM1      PA_15
+#define MOTOR_PWM2      PA_9
+#define MOTOR_PWM3      PA_8
+#define MOTOR_PWM4      PB_10
+#define MOTOR_PWM5      PB_3
+#define MOTOR_PWM6      PA_10
+
+#include "spi_setup.h"
+#include "motor.h"
+#include "encoder.h"
+#include "timer.h"
+#include "speed_pid.h"
+#include "target_position_cal.h"
+#include "Position.h"
+#include "pc_serial.h"
+#include "limit.h"
+//#include "SPI_TO_UART.h"
+
+
+void board_init()
+{
+    /*
+    set_data[1]=Speed_Igain;
+    set_data[2]=Speed_Pgain;
+    set_data[4]=Position_input_filter;
+    set_data[5]=position_Pgain1;
+    set_data[6]=position_Pgain2;
+    set_data[7]=position_Pgain3;
+    set_data[8]=offset;
+    */
+    limit_init();
+    SPI_INIT();
+    motor_init();
+    Serial_Init();
+    timer_init();
+    
+}
\ No newline at end of file

diff -r 000000000000 -r c21936a3520a mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file

diff -r 000000000000 -r c21936a3520a motor.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.h	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,51 @@
+#ifndef _MOTOR_H_
+#define _MOTOR_H_
+
+#include "FastPWM.h"
+
+
+
+FastPWM pwm1(MOTOR_PWM1, -1);
+FastPWM pwm2(MOTOR_PWM2, -1);
+FastPWM pwm3(MOTOR_PWM3, -1);
+FastPWM pwm4(MOTOR_PWM4, -1);
+FastPWM pwm5(MOTOR_PWM5, -1);
+//FastPWM pwm6(MOTOR_PWM6, -1);
+
+void motor_init()
+{
+    pwm1.period_us(2500);
+    pwm2.period_us(2500);
+    pwm3.period_us(2500);
+    pwm4.period_us(2500);
+    pwm5.period_us(2500);
+    //pwm6.period_us(2500);
+}
+
+void motor_power(int motor_num,double percent)
+{
+    percent=-percent;
+    double output=offset[motor_num];
+    if(percent<-500)
+        percent=-500;
+    if(percent>500)
+        percent=500;
+        
+        output = 1-(offset[motor_num] + percent)/2500;
+        
+        if(motor_num==0)
+            pwm1.write(output);
+        else if(motor_num==1)
+            pwm2.write(output);
+        else if(motor_num==2)
+            pwm3.write(output);
+        else if(motor_num==3)
+            pwm4.write(output);
+        else if(motor_num==4)
+            pwm5.write(output);
+        //else if(motor_num==5)
+        //    pwm6.write(output);
+}
+
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r c21936a3520a pc_serial.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pc_serial.h	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,316 @@
+#define debug1 0
+#define debug2 0 
+#define Start_com 1
+Serial pc(USBTX, USBRX);
+//Serial pc2(PB_6, PB_7);
+
+char check_pitch[6];
+char rx_buffer[100]={0,};
+char command[100]={0,};
+char tx_buffer[100]="txdat32768,32768,32768,32768,32768,32768\n";
+int tx_index = 0;
+int rx_index = 0;
+bool rx_flag = false;
+
+long data_recive=0;
+
+long bool_cnt=0;
+
+double set_data[100] = {0x00, }; 
+
+unsigned int cmd_roll, cmd_pitch, cmd_heave, cmd_sway, cmd_surge, cmd_yaw;
+
+double set_num = 0;
+
+void Start_Command();
+void Set_Arrangement();
+
+bool data_error_check();
+void char_save_arr(char c);
+//void Rx_Interrupt();
+void READ_and_SAVE();
+void Serial_Init();
+void Rx_Buffer_Clear();
+
+
+
+void comunication()
+{
+        
+        
+        /*
+        taget_speed=set_data[0];
+        Speed_Igain=set_data[1];
+        Speed_Pgain=set_data[2];
+        taget_position=set_data[3];
+        Position_input_filter=set_data[4];
+        position_Pgain1=set_data[5];
+        position_Pgain2=set_data[6];
+        position_Pgain3=set_data[7];
+        offset=set_data[8];
+        */
+        
+    READ_and_SAVE();
+    Start_Command();
+    
+}
+
+void Start_Command()
+{
+    bool com_error=false;
+    
+    unsigned int temp=0;
+
+    unsigned int data1=32768;
+    unsigned int data2=32768;
+    unsigned int data3=32768;
+    unsigned int data4=32768;
+    unsigned int data5=32768;
+    unsigned int data6=32768;
+    
+    com_error=data_error_check();
+    
+    if(com_error==false)
+    if((command[0] == 'S') && (command[1] == 't') && (command[2] == 'a') && (command[3] == 'r') && (command[4] == 't'))
+    {    
+   
+        int num=0;
+        for(int i=5; i<100;i++)
+        {
+
+            if(command[i]==',')
+            {
+                if(num==0)
+                    data1=temp;
+                if(num==1)
+                    data2=temp;
+                if(num==2)
+                    data3=temp;
+                if(num==3)
+                    data4=temp;
+                if(num==4)
+                    data5=temp;
+                    
+                temp=0;
+                num++;
+            }
+            else if(command[i]=='\n')
+            {
+                if(num==5)
+                    data6=temp;
+            }else
+                temp=temp*10+command[i]-0x30;
+        }
+        
+        cmd_roll     =  data1;
+        cmd_pitch    =  data2;
+        cmd_heave    =  data3;
+        cmd_sway     =  data4;
+        cmd_surge    =  data5;
+        cmd_yaw      =  data6;
+        
+        /*
+        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");
+        */
+    }
+
+}
+
+bool data_error_check() // ,아스키코드 44 숫자 아스키코드 0x30, 0x39
+{
+    bool error=false;
+    
+    
+    if(!((command[0] == 'S') && (command[1] == 't') && (command[2] == 'a') && (command[3] == 'r') && (command[4] == 't')))
+      error=true;
+      
+      
+    for(int i=5; i<41;i++)
+    {
+      if( command[i]>0x39 || command[i]<0x30 )
+      {
+          if((command[i]=='\n'))
+            break;
+            
+          if(!(command[i]==0x2C))
+          error=true;
+      }
+    }
+    
+    for(int i=41;i<100;i++)
+    {
+        if(!(command[i]==0))
+        error=true;
+    }
+      
+    for(int i=0;i<17;i++)
+    {
+        if(command[i]==0)
+        error=true;
+    }
+    
+    
+    return error;
+}
+
+
+// Set arrangement function
+void Set_Arrangement()
+{
+    bool set_error = false;
+    bool set_point = false;
+    double square_root = 0.1;
+    double error_temp = set_num;
+    
+    if((command[0] == 'S') && (command[1] == 'E') && (command[2] == 'T'))
+    {
+        for(int i = 3; i < 100; i++)
+        {            
+            // Error check
+            if(command[i] != '.' && command[i] < '0')
+            {
+                set_error = true;
+                break;
+            }
+            else if(command[i] != '.' && command[i] > '9')
+            {
+                set_error = true;
+                break;
+            }
+            
+            
+            if(command[i] == '\n')
+            {
+                break;
+            }
+            else if(command[i] == '.' && set_point == false)
+            {
+                set_point = true;
+            }
+            else if(set_point == false)
+            {
+                set_num = (set_num * 10) + (command[i] - 0x30);
+            }
+            else if(set_point == true)
+            {
+                set_num = set_num + ((command[i] - 0x30) * square_root);
+                square_root = square_root * 0.1;
+            }
+        }
+        
+        if(set_error == true)
+        {
+            set_num = error_temp;
+        }
+    }
+}     
+    
+
+bool tx_done=false;
+void READ_and_SAVE()
+{
+    
+    cs = 1;
+    char c;
+    tx_done=false;
+  for(int i=0; i<64; i++)
+  { 
+        wait_us(20);
+        c = spi1.write(255);
+        
+    if(c==255)
+    {
+        break;
+    }
+    else
+    {
+        //pc.printf("%d\r\n",c);
+        char_save_arr(c);
+    }
+  }   
+  cs = 0;
+  
+}
+
+
+void char_save_arr(char c)
+{
+    rx_buffer[rx_index]=c;
+    rx_index++;
+    if(rx_index>99)
+    rx_index=99;
+    if(c=='\n')
+    {
+        data_recive++;
+        for(int i =0; i<100;i++)
+        command[i]=rx_buffer[i];
+
+        Rx_Buffer_Clear();
+
+        rx_index=0;
+    }
+}
+
+
+
+
+// Interrupt functions
+/*
+void Rx_Interrupt()
+{
+    if(pc.readable())
+    {       
+        char c = pc.getc();
+        rx_buffer[rx_index] = c;
+        if(rx_buffer[rx_index] == '\n')
+        {
+            rx_flag = true;
+        }
+        rx_index++;
+        if(rx_index > 99)
+        {
+            rx_index = 99;
+        }
+    }
+    return;
+}
+*/
+// Basic serial communication functions
+
+void Serial_Init()
+{    
+    pc.format(8, Serial::None, 1);
+    pc.baud(2000000);
+    
+    //pc2.format(8, Serial::None, 1);
+    //pc2.baud(115200);
+    //pc.attach(&Rx_Interrupt, Serial::RxIrq);
+}
+
+
+void Rx_Buffer_Clear()
+{
+        for(int i = 0; i < 100; i++)
+        {
+            rx_buffer[i] = 0x00;
+        }
+}
+
+
+/*
+  if(com_error==false)
+            {
+                cmd_roll     =  data1;
+                cmd_pitch    =  data2;
+                cmd_heave    =  data3;
+                cmd_sway     =  data4;
+                cmd_surge    =  data5;
+                cmd_yaw      =  data6;
+            }
+            */
\ No newline at end of file

diff -r 000000000000 -r c21936a3520a speed_pid.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/speed_pid.h	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,43 @@
+double taget_speed[6]={0,};
+double error_speed[6]={0,};
+double Speed_PID_OUTPUT[6]={0,};
+
+void cal_speed_error()
+{
+    for(int i=0;i<6;i++)
+    error_speed[i]=filter_dif_encoder_data[i]-taget_speed[i];
+}
+
+double result_i[6]={0,};
+double filter_result_i[6]={0,};
+double output_i[6]={0,};
+
+void Speed_I()
+{
+    for(int i=0;i<6;i++)
+    {
+        if(filter_dif_encoder_data[i]<taget_speed[i])
+            result_i[i]+=1*Speed_Igain[i];
+        else if(filter_dif_encoder_data[i]>taget_speed[i])
+            result_i[i]-=1*Speed_Igain[i]; 
+            
+        filter_result_i[i] = filter_result_i[i]*(1-Speed_I_input_filter[i]) + result_i[i]*Speed_I_input_filter[i];
+        output_i[i] = filter_result_i[i];
+    }
+}
+double output_p[6]={0,};
+void Speed_P()
+{
+    for(int i=0;i<6;i++)
+        output_p[i] = - error_speed[i] * Speed_Pgain[i];
+}
+
+void Speed_PID()
+{
+    cal_speed_error();
+    Speed_I();
+    Speed_P();
+    
+    for(int i=0;i<6;i++)
+    Speed_PID_OUTPUT[i] = output_i[i] + output_p[i];
+}

diff -r 000000000000 -r c21936a3520a spi_setup.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/spi_setup.h	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,17 @@
+#include "LS7366.h"
+
+SPI spi1(SPI_MOSI, SPI_MISO, SPI_SCK);
+
+LS7366 encoder1(spi1, ENCODER_CS1);
+LS7366 encoder2(spi1, ENCODER_CS2);
+LS7366 encoder3(spi1, ENCODER_CS3);
+LS7366 encoder4(spi1, ENCODER_CS4);
+LS7366 encoder5(spi1, ENCODER_CS5);
+LS7366 encoder6(spi1, ENCODER_CS6);
+DigitalOut cs(PB_4);
+
+void SPI_INIT()
+{
+    spi1.format(8,0);
+    spi1.frequency(2000000);
+}
\ No newline at end of file

diff -r 000000000000 -r c21936a3520a target_position_cal.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/target_position_cal.h	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,77 @@
+double first_degree, second_degree, third_degree, four_degree, five_degree, six_degree;
+
+double motor1_degree=0;
+double motor2_degree=0;
+double motor3_degree=0;
+double motor4_degree=0;
+double motor5_degree=0;
+double motor6_degree=0;
+
+#define roll_gain 15 // roll 게인 ( 부호를 바꾸면 방향이 바뀐다.)
+#define pitch_gain 15 // pitch 게인 ( 부호를 바꾸면 방향이 바뀐다.)
+#define heave_gain 15 // heave_gain 게인 ( 부호를 바꾸면 방향이 바뀐다.)
+
+double origin_degree1=50;    // 모터1 기준위치 지정
+double origin_degree2=50;    // 모터2 기준위치 지정
+double origin_degree3=50;    // 모터3 기준위치 지정
+double origin_degree4=60;    // 모터2 기준위치 지정
+double origin_degree5=90;    // 모터3 기준위치 지정
+double origin_degree6=90;    // 모터3 기준위치 지정
+double cal_scale=0.7;  // 전체 게인 조절
+
+double cal_roll=0;
+double cal_pitch=0;
+double cal_heave=0;
+double cal_surge=0;
+double cal_yaw=0;
+double cal_sway=0;
+double cal_senter=0;
+
+double pitch_motor1, pitch_motor2, pitch_motor3;
+double roll_motor1, roll_motor2, roll_motor3;
+double heave_motor1, heave_motor2, heave_motor3;
+
+
+void taget_position_cal(double r,double p, double h,double sw,double su,double y) // before, cal(roll_gain*dou_roll,pitch_gain*dou_pitch,heave_gain*dou_heave,0,gain); / add, surge, yaw, sway
+{
+    
+    r   =   ((r     - 32768) / 1000) * 0.2746582;
+    p   =   ((p     - 32768) / 1000) * 0.2746582;
+    h   =   ((h     - 32768) / 1000) * 0.2746582;
+    su  =   ((su    - 32768) / 1000) * 0.2746582;
+    y   =   ((y     - 32768) / 1000) * 0.2746582;
+    sw  =   ((sw    - 32768) / 1000) * 0.2746582;
+
+    
+    r=roll_gain*r;
+    p=pitch_gain*p;
+    h=heave_gain*h;
+    
+    pitch_motor1 = cal_scale * (-p);
+    pitch_motor2 = cal_scale * (p);
+    pitch_motor3 = cal_scale * (p);
+    
+    roll_motor1 = 0;
+    roll_motor2 = (cal_scale * r);
+    roll_motor3 = (cal_scale * -r);
+    
+    heave_motor1 = (cal_scale * h);
+    heave_motor2 = (cal_scale * h);
+    heave_motor3 = (cal_scale * h);
+    
+    motor1_degree = cal_senter + pitch_motor1 + roll_motor1 + heave_motor1;
+    motor2_degree = cal_senter + pitch_motor2 + roll_motor2 + heave_motor2;
+    motor3_degree = cal_senter + pitch_motor3 + roll_motor3 + heave_motor3;
+    
+    motor4_degree = 18.66 * sw;                                   //회전비율조정(심툴즈 아웃풋 테스트 스로틀 맞춤)
+    motor5_degree = 9.33 * su-9.33 * y;                    //회전비율조정(심툴즈 아웃풋 테스트 스로틀 맞춤)
+    motor6_degree = 9.33 * su+9.33 * y;                    //회전비율조정(심툴즈 아웃풋 테스트 스로틀 맞춤)
+    
+    first_degree = motor1_degree+origin_degree1;
+    second_degree = motor2_degree+origin_degree2;
+    third_degree = motor3_degree+origin_degree3;
+    four_degree = motor4_degree+origin_degree4;
+    five_degree = motor5_degree+origin_degree5;
+    six_degree = motor6_degree+origin_degree6;
+    
+}
\ No newline at end of file

diff -r 000000000000 -r c21936a3520a timer.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/timer.h	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,40 @@
+
+Timer t;
+
+
+int delay_time=0;
+
+bool find_delay=false;
+
+uint32_t loop_time = 0;
+uint32_t ex_loop_time = 0;
+uint32_t dif_loop_time = 0;
+double loop_time_f = 0;
+
+void timer_init()
+{
+    //t.start();
+}
+
+
+void loop_time_run()
+{
+    loop_time = t.read_us();
+    dif_loop_time=loop_time-ex_loop_time;
+    ex_loop_time = loop_time;
+    
+    loop_time_f=(double)dif_loop_time;
+    
+    if(dif_loop_time==2000)
+            find_delay=true;
+        
+        if(find_delay==false)
+        {
+            if(dif_loop_time>2000)
+                delay_time--;
+            if(dif_loop_time<2000)
+                delay_time++;
+        }
+
+        wait_us(delay_time);
+}
\ No newline at end of file