Modbus example

#include <io>
#include <read>
#include <float>
#include <string>
#include <core>
#include <write>
#include <define>


new mas[16];
new mas2[8];
new mas3[6];
new mas4[50];
new interface = RS232;                

forward ModbusSend(adress, function, start, howMuch);
forward ModbusReceive();

main(){
  Init(interface, 19200,  WORDLENGTH_8, STOPBITS_1, PARITY_NONE);    
  while(1){
      ModbusSend2(15, 3, 5, 5);
      ModbusReceive();
      Delay(1000); 
      write_buf(interface, 15, mas4);
      Delay(1000);  
  }
}

ModbusSend(adress, function, start, howMuch){
new crc2;   
mas2[0] = adress;
mas2[1] = function;
mas2[2] = (start & 0xFF00) >> 8;
mas2[3] = start & 0xFF;
mas2[4] = (howMuch & 0xFF00) >> 8;
mas2[5] = howMuch & 0xFF;
crc2 = CRC16_2(6);
mas2[6] = crc2 & 0xFF;
mas2[7] = (crc2 & 0xFF00) >> 8;
write_buf(interface, sizeof(mas2), mas2); 
}

ModbusReceive(){
    if(buf_len(interface) != 0){
        new len;
        len = buf_len(interface);                                        
        read_buf(interface, len, mas4);
    }
}

CRC16_2(len)
{
    new i;
    new pos;
    new crc = 0xFFFF;

  for (pos = 0; pos < len; pos++)
  {
    crc ^= mas2[pos];          // XOR byte into least sig. byte of crc

    for (i = 8; i != 0; i--) {    // Loop over each bit
      if ((crc & 0x0001) != 0) {      // If the LSB is set
        crc >>= 1;                    // Shift right and XOR 0xA001
        crc ^= 0xA001;
      }
      else                            // Else LSB is not set
        crc >>= 1;                    // Just shift right
    }
  }
  return crc;
}