Diff: pc_serial.h

Revision:

0:e12eb40b9fef

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pc_serial.h	Thu Apr 23 00:38:16 2020 +0000
@@ -0,0 +1,341 @@
+#ifndef _PC_SERIAL_H_
+#define _PC_SERIAL_H_
+
+#define debug1 0
+#define debug2 0 
+#define Start_com 1
+
+Serial pc(USBTX, USBRX);
+
+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;
+
+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()
+{
+        
+        
+        for(int i=0;i<6;i++)
+        {
+            Speed_Pgain[i]=set_data[(i+1)*10+0];
+            Speed_Igain[i]=set_data[(i+1)*10+1];
+            position_Pgain[i]=set_data[(i+1)*10+2];
+            Position_input_filter[i]=set_data[(i+1)*10+3];
+            Speed_I_input_filter[i]=set_data[(i+1)*10+4];
+            offset[i]=set_data[(i+1)*10+5];
+        }
+        
+ 
+    READ_and_SAVE();
+    //pc.printf("%s,",command);
+    Set_Arrangement();
+    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;
+            break;
+        }
+      }
+    }
+    
+    for(int i=41;i<100;i++)
+    {
+        if(command[i]!=0)
+            error=true;
+            break;
+    }
+      
+    for(int i=0;i<17;i++)
+    {
+        if(command[i]==0)
+            error=true;
+            break;
+    }
+    
+    return error;
+}
+
+
+// Set arrangement function
+void Set_Arrangement()
+{
+    bool set_error = false;
+    bool set_point = false;
+    int set_index = 0;
+    double set_num = 0;
+    double square_root = 0.1;
+    
+    if((command[0] == 'S') && (command[1] == 'E') && (command[2] == 'T') && (command[5] == '='))
+    {
+        for(int i = 3; i < 5; i++)
+        {
+            if(command[i] < '0' || command[i] > '9')
+            {
+                set_error = true;
+                break;
+            }
+        }
+        if(set_error == false)
+        {
+            set_index = ((command[3] - 0x30) * 10) + (command[4] - 0x30);
+            
+            for(int i = 6; i < 100; i++)
+            {            
+                // Error check
+                if(command[i] != '.' && (command[i] < '0' || command[i] > '9'))
+                {
+                    if(command[i] != '\n')
+                    {
+                        set_error = true;
+                    }
+                    break;
+                }
+    
+                // Operation source         
+                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 == false) && (set_data[set_index] != set_num))
+        {
+            set_data[set_index] = set_num;
+            Erase_Flash();
+            Write_Flash('F');
+            Write_Flash('L');
+            Write_Flash('A');
+            Write_Flash('S');
+            Write_Flash('H');
+            for(int i = 0; i < 100; i++)
+            {
+                Write_Flash((uint32_t)set_data[i]);
+            }
+            pc.printf("SET%02d=%10.5f\n",set_index,set_data[set_index]);
+        }
+    }
+
+}     
+    
+
+bool tx_done=false;
+void READ_and_SAVE()
+{
+    
+    at_spi_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);
+    }
+  }   
+  at_spi_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();
+    }
+}
+
+
+
+
+// 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(500000);
+    
+    //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;
+        }
+        rx_index=0;
+}
+
+
+/*
+  if(com_error==false)
+            {
+                cmd_roll     =  data1;
+                cmd_pitch    =  data2;
+                cmd_heave    =  data3;
+                cmd_sway     =  data4;
+                cmd_surge    =  data5;
+                cmd_yaw      =  data6;
+            }
+            */
+            
+#endif
\ No newline at end of file