lee sanghun / Mbed 2 deprecated 202001131705

45

Dependencies:   mbed BufferedSerial LS7366LIB FastPWM

pc_serial.h@0:c21936a3520a, 2020-03-23 (annotated)

Committer:
lsh2205
Date:
Mon Mar 23 08:38:40 2020 +0000
Revision:
0:c21936a3520a
ss

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lsh2205 0:c21936a3520a 1 #define debug1 0
lsh2205 0:c21936a3520a 2 #define debug2 0
lsh2205 0:c21936a3520a 3 #define Start_com 1
lsh2205 0:c21936a3520a 4 Serial pc(USBTX, USBRX);
lsh2205 0:c21936a3520a 5 //Serial pc2(PB_6, PB_7);
lsh2205 0:c21936a3520a 6
lsh2205 0:c21936a3520a 7 char check_pitch[6];
lsh2205 0:c21936a3520a 8 char rx_buffer[100]={0,};
lsh2205 0:c21936a3520a 9 char command[100]={0,};
lsh2205 0:c21936a3520a 10 char tx_buffer[100]="txdat32768,32768,32768,32768,32768,32768\n";
lsh2205 0:c21936a3520a 11 int tx_index = 0;
lsh2205 0:c21936a3520a 12 int rx_index = 0;
lsh2205 0:c21936a3520a 13 bool rx_flag = false;
lsh2205 0:c21936a3520a 14
lsh2205 0:c21936a3520a 15 long data_recive=0;
lsh2205 0:c21936a3520a 16
lsh2205 0:c21936a3520a 17 long bool_cnt=0;
lsh2205 0:c21936a3520a 18
lsh2205 0:c21936a3520a 19 double set_data[100] = {0x00, };
lsh2205 0:c21936a3520a 20
lsh2205 0:c21936a3520a 21 unsigned int cmd_roll, cmd_pitch, cmd_heave, cmd_sway, cmd_surge, cmd_yaw;
lsh2205 0:c21936a3520a 22
lsh2205 0:c21936a3520a 23 double set_num = 0;
lsh2205 0:c21936a3520a 24
lsh2205 0:c21936a3520a 25 void Start_Command();
lsh2205 0:c21936a3520a 26 void Set_Arrangement();
lsh2205 0:c21936a3520a 27
lsh2205 0:c21936a3520a 28 bool data_error_check();
lsh2205 0:c21936a3520a 29 void char_save_arr(char c);
lsh2205 0:c21936a3520a 30 //void Rx_Interrupt();
lsh2205 0:c21936a3520a 31 void READ_and_SAVE();
lsh2205 0:c21936a3520a 32 void Serial_Init();
lsh2205 0:c21936a3520a 33 void Rx_Buffer_Clear();
lsh2205 0:c21936a3520a 34
lsh2205 0:c21936a3520a 35
lsh2205 0:c21936a3520a 36
lsh2205 0:c21936a3520a 37 void comunication()
lsh2205 0:c21936a3520a 38 {
lsh2205 0:c21936a3520a 39
lsh2205 0:c21936a3520a 40
lsh2205 0:c21936a3520a 41 /*
lsh2205 0:c21936a3520a 42 taget_speed=set_data[0];
lsh2205 0:c21936a3520a 43 Speed_Igain=set_data[1];
lsh2205 0:c21936a3520a 44 Speed_Pgain=set_data[2];
lsh2205 0:c21936a3520a 45 taget_position=set_data[3];
lsh2205 0:c21936a3520a 46 Position_input_filter=set_data[4];
lsh2205 0:c21936a3520a 47 position_Pgain1=set_data[5];
lsh2205 0:c21936a3520a 48 position_Pgain2=set_data[6];
lsh2205 0:c21936a3520a 49 position_Pgain3=set_data[7];
lsh2205 0:c21936a3520a 50 offset=set_data[8];
lsh2205 0:c21936a3520a 51 */
lsh2205 0:c21936a3520a 52
lsh2205 0:c21936a3520a 53 READ_and_SAVE();
lsh2205 0:c21936a3520a 54 Start_Command();
lsh2205 0:c21936a3520a 55
lsh2205 0:c21936a3520a 56 }
lsh2205 0:c21936a3520a 57
lsh2205 0:c21936a3520a 58 void Start_Command()
lsh2205 0:c21936a3520a 59 {
lsh2205 0:c21936a3520a 60 bool com_error=false;
lsh2205 0:c21936a3520a 61
lsh2205 0:c21936a3520a 62 unsigned int temp=0;
lsh2205 0:c21936a3520a 63
lsh2205 0:c21936a3520a 64 unsigned int data1=32768;
lsh2205 0:c21936a3520a 65 unsigned int data2=32768;
lsh2205 0:c21936a3520a 66 unsigned int data3=32768;
lsh2205 0:c21936a3520a 67 unsigned int data4=32768;
lsh2205 0:c21936a3520a 68 unsigned int data5=32768;
lsh2205 0:c21936a3520a 69 unsigned int data6=32768;
lsh2205 0:c21936a3520a 70
lsh2205 0:c21936a3520a 71 com_error=data_error_check();
lsh2205 0:c21936a3520a 72
lsh2205 0:c21936a3520a 73 if(com_error==false)
lsh2205 0:c21936a3520a 74 if((command[0] == 'S') && (command[1] == 't') && (command[2] == 'a') && (command[3] == 'r') && (command[4] == 't'))
lsh2205 0:c21936a3520a 75 {
lsh2205 0:c21936a3520a 76
lsh2205 0:c21936a3520a 77 int num=0;
lsh2205 0:c21936a3520a 78 for(int i=5; i<100;i++)
lsh2205 0:c21936a3520a 79 {
lsh2205 0:c21936a3520a 80
lsh2205 0:c21936a3520a 81 if(command[i]==',')
lsh2205 0:c21936a3520a 82 {
lsh2205 0:c21936a3520a 83 if(num==0)
lsh2205 0:c21936a3520a 84 data1=temp;
lsh2205 0:c21936a3520a 85 if(num==1)
lsh2205 0:c21936a3520a 86 data2=temp;
lsh2205 0:c21936a3520a 87 if(num==2)
lsh2205 0:c21936a3520a 88 data3=temp;
lsh2205 0:c21936a3520a 89 if(num==3)
lsh2205 0:c21936a3520a 90 data4=temp;
lsh2205 0:c21936a3520a 91 if(num==4)
lsh2205 0:c21936a3520a 92 data5=temp;
lsh2205 0:c21936a3520a 93
lsh2205 0:c21936a3520a 94 temp=0;
lsh2205 0:c21936a3520a 95 num++;
lsh2205 0:c21936a3520a 96 }
lsh2205 0:c21936a3520a 97 else if(command[i]=='\n')
lsh2205 0:c21936a3520a 98 {
lsh2205 0:c21936a3520a 99 if(num==5)
lsh2205 0:c21936a3520a 100 data6=temp;
lsh2205 0:c21936a3520a 101 }else
lsh2205 0:c21936a3520a 102 temp=temp*10+command[i]-0x30;
lsh2205 0:c21936a3520a 103 }
lsh2205 0:c21936a3520a 104
lsh2205 0:c21936a3520a 105 cmd_roll = data1;
lsh2205 0:c21936a3520a 106 cmd_pitch = data2;
lsh2205 0:c21936a3520a 107 cmd_heave = data3;
lsh2205 0:c21936a3520a 108 cmd_sway = data4;
lsh2205 0:c21936a3520a 109 cmd_surge = data5;
lsh2205 0:c21936a3520a 110 cmd_yaw = data6;
lsh2205 0:c21936a3520a 111
lsh2205 0:c21936a3520a 112 /*
lsh2205 0:c21936a3520a 113 pc.printf("%d,",cmd_roll);
lsh2205 0:c21936a3520a 114 pc.printf("%d,",cmd_pitch);
lsh2205 0:c21936a3520a 115 pc.printf("%d,",cmd_heave);
lsh2205 0:c21936a3520a 116 pc.printf("%d,",cmd_sway);
lsh2205 0:c21936a3520a 117 pc.printf("%d,",cmd_surge);
lsh2205 0:c21936a3520a 118 pc.printf("%d,",cmd_yaw);
lsh2205 0:c21936a3520a 119 pc.printf("\n");
lsh2205 0:c21936a3520a 120 */
lsh2205 0:c21936a3520a 121 }
lsh2205 0:c21936a3520a 122
lsh2205 0:c21936a3520a 123 }
lsh2205 0:c21936a3520a 124
lsh2205 0:c21936a3520a 125 bool data_error_check() // ,아스키코드 44 숫자 아스키코드 0x30, 0x39
lsh2205 0:c21936a3520a 126 {
lsh2205 0:c21936a3520a 127 bool error=false;
lsh2205 0:c21936a3520a 128
lsh2205 0:c21936a3520a 129
lsh2205 0:c21936a3520a 130 if(!((command[0] == 'S') && (command[1] == 't') && (command[2] == 'a') && (command[3] == 'r') && (command[4] == 't')))
lsh2205 0:c21936a3520a 131 error=true;
lsh2205 0:c21936a3520a 132
lsh2205 0:c21936a3520a 133
lsh2205 0:c21936a3520a 134 for(int i=5; i<41;i++)
lsh2205 0:c21936a3520a 135 {
lsh2205 0:c21936a3520a 136 if( command[i]>0x39 || command[i]<0x30 )
lsh2205 0:c21936a3520a 137 {
lsh2205 0:c21936a3520a 138 if((command[i]=='\n'))
lsh2205 0:c21936a3520a 139 break;
lsh2205 0:c21936a3520a 140
lsh2205 0:c21936a3520a 141 if(!(command[i]==0x2C))
lsh2205 0:c21936a3520a 142 error=true;
lsh2205 0:c21936a3520a 143 }
lsh2205 0:c21936a3520a 144 }
lsh2205 0:c21936a3520a 145
lsh2205 0:c21936a3520a 146 for(int i=41;i<100;i++)
lsh2205 0:c21936a3520a 147 {
lsh2205 0:c21936a3520a 148 if(!(command[i]==0))
lsh2205 0:c21936a3520a 149 error=true;
lsh2205 0:c21936a3520a 150 }
lsh2205 0:c21936a3520a 151
lsh2205 0:c21936a3520a 152 for(int i=0;i<17;i++)
lsh2205 0:c21936a3520a 153 {
lsh2205 0:c21936a3520a 154 if(command[i]==0)
lsh2205 0:c21936a3520a 155 error=true;
lsh2205 0:c21936a3520a 156 }
lsh2205 0:c21936a3520a 157
lsh2205 0:c21936a3520a 158
lsh2205 0:c21936a3520a 159 return error;
lsh2205 0:c21936a3520a 160 }
lsh2205 0:c21936a3520a 161
lsh2205 0:c21936a3520a 162
lsh2205 0:c21936a3520a 163 // Set arrangement function
lsh2205 0:c21936a3520a 164 void Set_Arrangement()
lsh2205 0:c21936a3520a 165 {
lsh2205 0:c21936a3520a 166 bool set_error = false;
lsh2205 0:c21936a3520a 167 bool set_point = false;
lsh2205 0:c21936a3520a 168 double square_root = 0.1;
lsh2205 0:c21936a3520a 169 double error_temp = set_num;
lsh2205 0:c21936a3520a 170
lsh2205 0:c21936a3520a 171 if((command[0] == 'S') && (command[1] == 'E') && (command[2] == 'T'))
lsh2205 0:c21936a3520a 172 {
lsh2205 0:c21936a3520a 173 for(int i = 3; i < 100; i++)
lsh2205 0:c21936a3520a 174 {
lsh2205 0:c21936a3520a 175 // Error check
lsh2205 0:c21936a3520a 176 if(command[i] != '.' && command[i] < '0')
lsh2205 0:c21936a3520a 177 {
lsh2205 0:c21936a3520a 178 set_error = true;
lsh2205 0:c21936a3520a 179 break;
lsh2205 0:c21936a3520a 180 }
lsh2205 0:c21936a3520a 181 else if(command[i] != '.' && command[i] > '9')
lsh2205 0:c21936a3520a 182 {
lsh2205 0:c21936a3520a 183 set_error = true;
lsh2205 0:c21936a3520a 184 break;
lsh2205 0:c21936a3520a 185 }
lsh2205 0:c21936a3520a 186
lsh2205 0:c21936a3520a 187
lsh2205 0:c21936a3520a 188 if(command[i] == '\n')
lsh2205 0:c21936a3520a 189 {
lsh2205 0:c21936a3520a 190 break;
lsh2205 0:c21936a3520a 191 }
lsh2205 0:c21936a3520a 192 else if(command[i] == '.' && set_point == false)
lsh2205 0:c21936a3520a 193 {
lsh2205 0:c21936a3520a 194 set_point = true;
lsh2205 0:c21936a3520a 195 }
lsh2205 0:c21936a3520a 196 else if(set_point == false)
lsh2205 0:c21936a3520a 197 {
lsh2205 0:c21936a3520a 198 set_num = (set_num * 10) + (command[i] - 0x30);
lsh2205 0:c21936a3520a 199 }
lsh2205 0:c21936a3520a 200 else if(set_point == true)
lsh2205 0:c21936a3520a 201 {
lsh2205 0:c21936a3520a 202 set_num = set_num + ((command[i] - 0x30) * square_root);
lsh2205 0:c21936a3520a 203 square_root = square_root * 0.1;
lsh2205 0:c21936a3520a 204 }
lsh2205 0:c21936a3520a 205 }
lsh2205 0:c21936a3520a 206
lsh2205 0:c21936a3520a 207 if(set_error == true)
lsh2205 0:c21936a3520a 208 {
lsh2205 0:c21936a3520a 209 set_num = error_temp;
lsh2205 0:c21936a3520a 210 }
lsh2205 0:c21936a3520a 211 }
lsh2205 0:c21936a3520a 212 }
lsh2205 0:c21936a3520a 213
lsh2205 0:c21936a3520a 214
lsh2205 0:c21936a3520a 215 bool tx_done=false;
lsh2205 0:c21936a3520a 216 void READ_and_SAVE()
lsh2205 0:c21936a3520a 217 {
lsh2205 0:c21936a3520a 218
lsh2205 0:c21936a3520a 219 cs = 1;
lsh2205 0:c21936a3520a 220 char c;
lsh2205 0:c21936a3520a 221 tx_done=false;
lsh2205 0:c21936a3520a 222 for(int i=0; i<64; i++)
lsh2205 0:c21936a3520a 223 {
lsh2205 0:c21936a3520a 224 wait_us(20);
lsh2205 0:c21936a3520a 225 c = spi1.write(255);
lsh2205 0:c21936a3520a 226
lsh2205 0:c21936a3520a 227 if(c==255)
lsh2205 0:c21936a3520a 228 {
lsh2205 0:c21936a3520a 229 break;
lsh2205 0:c21936a3520a 230 }
lsh2205 0:c21936a3520a 231 else
lsh2205 0:c21936a3520a 232 {
lsh2205 0:c21936a3520a 233 //pc.printf("%d\r\n",c);
lsh2205 0:c21936a3520a 234 char_save_arr(c);
lsh2205 0:c21936a3520a 235 }
lsh2205 0:c21936a3520a 236 }
lsh2205 0:c21936a3520a 237 cs = 0;
lsh2205 0:c21936a3520a 238
lsh2205 0:c21936a3520a 239 }
lsh2205 0:c21936a3520a 240
lsh2205 0:c21936a3520a 241
lsh2205 0:c21936a3520a 242 void char_save_arr(char c)
lsh2205 0:c21936a3520a 243 {
lsh2205 0:c21936a3520a 244 rx_buffer[rx_index]=c;
lsh2205 0:c21936a3520a 245 rx_index++;
lsh2205 0:c21936a3520a 246 if(rx_index>99)
lsh2205 0:c21936a3520a 247 rx_index=99;
lsh2205 0:c21936a3520a 248 if(c=='\n')
lsh2205 0:c21936a3520a 249 {
lsh2205 0:c21936a3520a 250 data_recive++;
lsh2205 0:c21936a3520a 251 for(int i =0; i<100;i++)
lsh2205 0:c21936a3520a 252 command[i]=rx_buffer[i];
lsh2205 0:c21936a3520a 253
lsh2205 0:c21936a3520a 254 Rx_Buffer_Clear();
lsh2205 0:c21936a3520a 255
lsh2205 0:c21936a3520a 256 rx_index=0;
lsh2205 0:c21936a3520a 257 }
lsh2205 0:c21936a3520a 258 }
lsh2205 0:c21936a3520a 259
lsh2205 0:c21936a3520a 260
lsh2205 0:c21936a3520a 261
lsh2205 0:c21936a3520a 262
lsh2205 0:c21936a3520a 263 // Interrupt functions
lsh2205 0:c21936a3520a 264 /*
lsh2205 0:c21936a3520a 265 void Rx_Interrupt()
lsh2205 0:c21936a3520a 266 {
lsh2205 0:c21936a3520a 267 if(pc.readable())
lsh2205 0:c21936a3520a 268 {
lsh2205 0:c21936a3520a 269 char c = pc.getc();
lsh2205 0:c21936a3520a 270 rx_buffer[rx_index] = c;
lsh2205 0:c21936a3520a 271 if(rx_buffer[rx_index] == '\n')
lsh2205 0:c21936a3520a 272 {
lsh2205 0:c21936a3520a 273 rx_flag = true;
lsh2205 0:c21936a3520a 274 }
lsh2205 0:c21936a3520a 275 rx_index++;
lsh2205 0:c21936a3520a 276 if(rx_index > 99)
lsh2205 0:c21936a3520a 277 {
lsh2205 0:c21936a3520a 278 rx_index = 99;
lsh2205 0:c21936a3520a 279 }
lsh2205 0:c21936a3520a 280 }
lsh2205 0:c21936a3520a 281 return;
lsh2205 0:c21936a3520a 282 }
lsh2205 0:c21936a3520a 283 */
lsh2205 0:c21936a3520a 284 // Basic serial communication functions
lsh2205 0:c21936a3520a 285
lsh2205 0:c21936a3520a 286 void Serial_Init()
lsh2205 0:c21936a3520a 287 {
lsh2205 0:c21936a3520a 288 pc.format(8, Serial::None, 1);
lsh2205 0:c21936a3520a 289 pc.baud(2000000);
lsh2205 0:c21936a3520a 290
lsh2205 0:c21936a3520a 291 //pc2.format(8, Serial::None, 1);
lsh2205 0:c21936a3520a 292 //pc2.baud(115200);
lsh2205 0:c21936a3520a 293 //pc.attach(&Rx_Interrupt, Serial::RxIrq);
lsh2205 0:c21936a3520a 294 }
lsh2205 0:c21936a3520a 295
lsh2205 0:c21936a3520a 296
lsh2205 0:c21936a3520a 297 void Rx_Buffer_Clear()
lsh2205 0:c21936a3520a 298 {
lsh2205 0:c21936a3520a 299 for(int i = 0; i < 100; i++)
lsh2205 0:c21936a3520a 300 {
lsh2205 0:c21936a3520a 301 rx_buffer[i] = 0x00;
lsh2205 0:c21936a3520a 302 }
lsh2205 0:c21936a3520a 303 }
lsh2205 0:c21936a3520a 304
lsh2205 0:c21936a3520a 305
lsh2205 0:c21936a3520a 306 /*
lsh2205 0:c21936a3520a 307 if(com_error==false)
lsh2205 0:c21936a3520a 308 {
lsh2205 0:c21936a3520a 309 cmd_roll = data1;
lsh2205 0:c21936a3520a 310 cmd_pitch = data2;
lsh2205 0:c21936a3520a 311 cmd_heave = data3;
lsh2205 0:c21936a3520a 312 cmd_sway = data4;
lsh2205 0:c21936a3520a 313 cmd_surge = data5;
lsh2205 0:c21936a3520a 314 cmd_yaw = data6;
lsh2205 0:c21936a3520a 315 }
lsh2205 0:c21936a3520a 316 */