File content as of revision 0:c21936a3520a:

#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;
            }
            */