diff --git a/Arduino/ModbusTest/Modbus/README.md b/Arduino/ModbusTest/Modbus/README.md new file mode 100644 index 00000000..11e736d1 --- /dev/null +++ b/Arduino/ModbusTest/Modbus/README.md @@ -0,0 +1,2 @@ +# Modbus +This library using for generate and communication with Modbus RTU protocol diff --git a/Arduino/ModbusTest/Modbus/examples/mqtt2Modbus.ino b/Arduino/ModbusTest/Modbus/examples/mqtt2Modbus.ino new file mode 100644 index 00000000..5d113cfa --- /dev/null +++ b/Arduino/ModbusTest/Modbus/examples/mqtt2Modbus.ino @@ -0,0 +1,126 @@ +#include +#include "Modbus.h" +#include "PubSubClient.h" + +#define TXPIN 17 +#define RXPIN 16 +#define MODE 5 + +WiFiClient client; +PubSubClient mqtt(client); +Modbus modbus(Serial1); + +char mac[16]; +char INVREQ[] = "INVREQ/000000000000"; +char INVRES[] = "INVRES/000000000000"; +char INVRAW[] = "INVRAW/000000000000"; + + + +void callback(char *topic, uint8_t *payload, unsigned int len){ + + +} + + +void mqttInit(){ + mqtt.setBufferSize(2048); + mqtt.setServer("test.com", 1883); + mqtt.setCallback(callback); + mqtt.connect("","username","password"); + mqtt.subscribe(INVREQ); +} + +void setup() { + // put your setup code here, to run once: + + Serial.begin(115200); + Serial1.begin(9600, SERIAL_8N1,RXPIN, TXPIN); + modbus.init(MODE); + Serial.println("\n\nConnecting to IoT"); + WiFi.begin("IoT","iot@2022"); + while (WiFi.status() != WL_CONNECTED) { + Serial.print("."); + delay(500); + } + Serial.println("\n\nWiFi Connected"); + Serial.print("IP:"); + Serial.println(WiFi.localIP()); + + uint64_t chipid = ESP.getEfuseMac(); + sprintf(mac, "%04X%08X", (uint16_t)(chipid>>32),(uint32_t)chipid); + for(int i = 0;i < 12;i++){ + INVREQ[i+7] = mac[i]; + INVRES[i+7] = mac[i]; + INVRAW[i+7] = mac[i]; + } + + Serial.println(INVREQ); + Serial.println(INVRES); + Serial.println(INVRAW); + mqttInit(); + + + +} + +long timer = 0; + + +void loop() { + // put your main code here, to run repeatedly: + + if (millis() - timer > 2000) { + + + if(modbus.requestFrom(0x01, 0x04, 0x00, 79) > 0) { + + + byte raw[200]; + uint8_t len; + + modbus.RxRaw(raw, len); + long PV_V = modbus.uint16(1) / 10; + long PV_P = modbus.uint32(3); + long Load_P = modbus.uint32(9) / 10; + float Load = modbus.uint16(27)/10.0; + int SOC = modbus.uint16(18); + float GridV = modbus.uint16(20)/10.0; + float OutV = modbus.uint16(22)/10.0; + float OutA = modbus.uint16(34)/10.0; + float BAT_D = modbus.uint32(60) / 10.0; + float BAT_T = modbus.uint32(62) / 10.0; + long GRID_IN = modbus.uint32(36) / 10; + long BAT_DIS = modbus.uint32(73) / 10; + long BATCD = (long)modbus.uint32(77) / 10; + String rawStr = ""; + for (int i =0; i < len; i++) { + char txt[3]; + sprintf(txt, "%02X",raw[i]); + rawStr += String(txt); + // Serial.printf("%02X", raw[i]); + } + Serial.println(rawStr); + Serial.printf("PV_V: %4d ", PV_V); + Serial.printf("PV_P: %4d ", PV_P); + Serial.printf("LOAD: %3.1fV %3.1fA %4dW %4.1f%% ",OutV,OutA, Load_P,Load); + + Serial.printf("SOC: %2d%% ", SOC); + Serial.printf("GRID_V: %3.1f ", GridV); + Serial.printf("BAT TODAY: %2.1f/%0.1f Kwh ", BAT_D,BAT_T); + Serial.printf("GRID_IN: %4d W BAT_DIS: %4d W ", GRID_IN, BAT_DIS); + Serial.printf("BAT %S %4d W", (BATCD > 0? "DIS": (BATCD < 0? "CH ": "NA")), BATCD); + Serial.println(); + if(mqtt.connected()){ + + //Serial.println("Publish message"); + mqtt.publish(INVRES,rawStr.c_str()); + mqtt.publish(INVRAW,raw,len); + }else { + Serial.println("Not Connect"); + } + } + timer = millis(); + } + mqtt.loop(); +} diff --git a/Arduino/ModbusTest/Modbus/library.json b/Arduino/ModbusTest/Modbus/library.json new file mode 100644 index 00000000..5d03f20a --- /dev/null +++ b/Arduino/ModbusTest/Modbus/library.json @@ -0,0 +1,21 @@ +{ + "name": "Modbus", + "description": "Modbus Communication Interface ", + "keywords": "Modbus, RS485, RTU", + "authors": { + "name": "uldara", + "url": "https://github.com/uldara", + "maintainer": true + }, + "repository": { + "type": "git", + "url": "https://github.com/uldara1/Modbus.git" + }, + "version": "1.0.1", + "frameworks": ["arduino", "espidf"], + "platforms": ["espressif32", "espressif8266"], + "headers": "Modbus.h", + "build": { + "libArchive": false + } +} diff --git a/Arduino/ModbusTest/Modbus/library.properties b/Arduino/ModbusTest/Modbus/library.properties new file mode 100644 index 00000000..acae1a6c --- /dev/null +++ b/Arduino/ModbusTest/Modbus/library.properties @@ -0,0 +1,10 @@ +name=Modbus +version=1.0.1 +author=UL DARA +maintainer=UL DARA +sentence=Modbus RTU +paragraph=Support Modbus RTU protocol with Serial Port and TCP. +category=Communication +url=https://github.com/uldara1/Modbus +includes=Modbus.h +architecture=esp32,esp8266,arduino diff --git a/Arduino/ModbusTest/Modbus/license.txt b/Arduino/ModbusTest/Modbus/license.txt new file mode 100644 index 00000000..9f616729 --- /dev/null +++ b/Arduino/ModbusTest/Modbus/license.txt @@ -0,0 +1,7 @@ +vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvStartvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv +Software License Agreement (FreeBSD License) + +Copyright (c) 2023 Bodmer (https://github.com/uldara1) + +All rights reserved. +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^End^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/Arduino/ModbusTest/Modbus/src/Modbus.cpp b/Arduino/ModbusTest/Modbus/src/Modbus.cpp new file mode 100644 index 00000000..78a5426f --- /dev/null +++ b/Arduino/ModbusTest/Modbus/src/Modbus.cpp @@ -0,0 +1,424 @@ +#include +#include + +Modbus::Modbus() +{ + this->s = NULL; + this->mode_ = -1; +} +Modbus::Modbus(HardwareSerial &st) +{ + this->s = &st; +} + + +bool Modbus::init(int mode, bool en_log) +{ + this->mode_ = mode; + this->log = en_log; + pinMode(mode_,OUTPUT); + digitalWrite(mode_, 0); + + return true; +} + +void Modbus::setTimeout(uint16_t timeout) +{ + timeout_ = timeout; +} + +byte Modbus::byteRead(int index) +{ + return rawRx[index+3]; +} + +int Modbus::blockRead(int index) +{ + return ((dataRx[index*2] << 8) | dataRx[index*2+1]); +} + +int Modbus::coilRead(int address){ + + return coilRead(SlaveID,address); +} + +int Modbus::coilRead(int id, int address){ + if(requestFrom(id,Coil_Register,address,1)) + { + byte x = byteRead(0); + return bitRead(x,0); + }else + { + return -1; + } +} + +int Modbus::discreteInputRead(int address) +{ + return discreteInputRead(SlaveID,address); +} + +int Modbus::discreteInputRead(int id, int address) +{ + if(requestFrom(id,Discret_Register,address,1)) + { + byte x = byteRead(0); + return bitRead(x,0); + }else + { + return -1; + } +} + +long Modbus::holdingRegisterRead(int address) +{ + return holdingRegisterRead(SlaveID, address, 1); +} + +long Modbus::holdingRegisterRead(int id, int address, int block) +{ + if(block > 2){block = 2;} + if(requestFrom(SlaveID, Holding_Register, address, block)) + { + if(block == 2) + { + return (blockRead(0) << 16 | blockRead(1)); + } + else{ + return blockRead(0); + } + } + else{ + return -1; + } + +} + +long Modbus::inputRegisterRead(int address) +{ + return inputRegisterRead(SlaveID , address, 1); +} + +long Modbus::inputRegisterRead(int id, int address, int block) +{ + if(block > 2){block = 2;} + if(requestFrom(id, Input_Register,address,block)) + { + if(block == 2) + { + return (blockRead(0) << 16 | blockRead(1)); + } + else{ + return blockRead(0); + } + } + else + { + return -1; + } +} + + + + + + +int Modbus::requestFrom(int slaveId, int type, int address,int nb) +{ + + // address = address - 1; + int crc ; + txout[0] = slaveId; + txout[1] = type; + txout[2] = address >> 8; + txout[3] = address; + txout[4] = nb >> 8; + txout[5] = nb; + crc = this->CheckCRC(txout,6); + txout[6] = crc ; + txout[7] = crc >> 8; + + + if(log){ + Serial.print("TX: "); + for(int i =0; i < 8; i++) + { + Serial.printf("%02X ",txout[i] ); + } + Serial.print("\t"); + } + + digitalWrite(mode_,1); + delay(1); + this->s->write(txout,8); + this->s->flush(); + digitalWrite(mode_,0); + delay(1); + uint32_t t = millis(); + lenRx = 0; + datalen = 0; + int ll = 0; + int rx; + byte found = 0; + + while((millis() - t) < timeout_){ + if(this->s->available()) + { + rx = this->s->read(); + t = millis(); + + if(found == 0) + { + if(txout[ll] == rx){ll++;}else{ll = 0;} + if(ll == 2) + { + found = 1; + } + } + else if(found == 1){ + // Serial.print("Len: "); + // Serial.println(rx,DEC); + rawRx[0] = txout[0]; + rawRx[1] = txout[1]; + rawRx[2] = rx; + lenRx = 3; + found = 2; + } + else if(found == 2) + { + this->rawRx[lenRx++] = rx; + if(lenRx >= rawRx[2] + 5) { break; } + } + + } + + + } + + if(log){ + Serial.print("RX: "); + for(int i =0; i < lenRx; i++) + { + Serial.printf("%02X ",rawRx[i] ); + } + Serial.println(); + } + + if(lenRx > 2){ + int crc1 = rawRx[lenRx - 1] <<8 | rawRx[lenRx - 2]; + int crc2 = CheckCRC(rawRx, lenRx - 2); + //Serial.printf("CRC1: %04X CRC2: %04X\n",crc1, crc2); + + if(crc1 == crc2) + { + + datalen = rawRx[2]; + + // for(int i = 0; i < datalen;i++){ + // dataRx[i] = rawRx[i+3]; + // } + return datalen; + } + else{ return -1; } + }else{ + return -1; + } +} + + + + + + + +int Modbus::ReadCoilReg(int add) +{ + return ReadCoilReg(1, add, 1); +} + +int Modbus::ReadCoilReg(int slaveId, int add) +{ + return ReadCoilReg( slaveId, add, 1); +} + +int Modbus::ReadCoilReg(int slaveId, int add, int nbit) +{ + if(requestFrom(slaveId,Coil_Register,add,nbit)) + { + return byteRead(0); + }else + { + return -1; + } + +} + +int Modbus::ReadDiscretReg(int add) +{ + return ReadDiscretReg(1,add,1); +} + +int Modbus::ReadDiscretReg(int slaveId, int add) +{ + return ReadDiscretReg(slaveId,add,1); +} + +int Modbus::ReadDiscretReg(int slaveId, int add, int nbit) +{ + if(requestFrom(slaveId,Discret_Register,add,nbit)) { + return byteRead(0); + } + else { + return -1; + } +} + +int Modbus::ReadHoldingReg(int add) +{ + return 0; +} + +int Modbus::ReadHoldingReg(int slaveId, int add) +{ + return 0; +} + +int Modbus::ReadHoldingReg(int slaveId, int add, int nbyte) +{ + return 0; +} + +int Modbus::ReadInputReg(int add) +{ + return 0; +} + +int Modbus::ReadInputReg(int slaveId, int add) +{ + return 0; +} + +int Modbus::ReadInputReg(int slaveId, int add, int nbyte) +{ + return 0; +} + +int8_t Modbus::uint8(int add) +{ + return rawRx[add*2+3]; +} + + + + +uint16_t Modbus::uint16(int add) +{ + int add_ = (add)*2 + 3; + + return (rawRx[add_] << 8 | rawRx[add_+1]); +} + +uint32_t Modbus::uint32(int add, bool byteHL) +{ + uint32_t val ; + if (byteHL) + { + val = uint16(add) << 16 | uint16(add+1); + } + else + { + val = uint16(add+1)<< 16 | uint16(add); + } + return val; +} + +void Modbus::RxRaw(byte *raw, uint8_t &rlen) +{ + + + for(int i =0; i < lenRx; i++) + { + raw[i] = rawRx[i]; + // if(rawRx[i] < 16) + // { + // Serial.print("0"); + // } + // Serial.print(rawRx[i],HEX); + } + rlen = this->lenRx; + // Serial.println(rlen); +} + + +void Modbus::TxRaw(byte *raw, uint8_t &rlen) +{ + + + for(int i =0; i < 8; i++) + { + raw[i] = txout[i]; + // if(rawRx[i] < 16) + // { + // Serial.print("0"); + // } + // Serial.print(rawRx[i],HEX); + } + rlen = 8; + // Serial.println(rlen); +} + + +int Modbus::CheckCRC(byte *buf, int len) +{ + int nominal = 0xA001; + int crc = 0xFFFF; + unsigned char pos,i; + + for ( pos = 0; pos < len; pos++) { + crc ^= (unsigned int)buf[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 ^= nominal; + } + else // Else LSB is not set + crc >>= 1; // Just shift right + } + } + // Note, this number has low and high bytes swapped, so use it accordingly (or swap bytes) + return crc; +} + + + +int Modbus::holdingRegisterWrite(int id, int address, uint16_t value) +{ + int crc ; + + // form signal + txout[0] = id; + txout[1] = Write_Holding_Register; + txout[2] = address >> 8; + txout[3] = address; + txout[4] = value >> 8; + txout[5] = value; + crc = this->CheckCRC(txout,6); + txout[6] = crc ; + txout[7] = crc >> 8; + + // send signal + digitalWrite(mode_,1); + delay(1); + this->s->write(txout,8); + this->s->flush(); + digitalWrite(mode_,0); + delay(1); + + + return 1; + + + + +} \ No newline at end of file diff --git a/Arduino/ModbusTest/Modbus/src/Modbus.h b/Arduino/ModbusTest/Modbus/src/Modbus.h new file mode 100644 index 00000000..a861615c --- /dev/null +++ b/Arduino/ModbusTest/Modbus/src/Modbus.h @@ -0,0 +1,101 @@ +#ifndef MODBUS_H +#define MODBUS_H + +// #ifdef MODEBUS_LOG +// #define MODEBUS_LOG + + +#include +#include +#include +using namespace std; + +class Modbus +{ +private: + /* data */ + bool log = false; + int mode_ = -1; + uint32_t timeout_ = 100; + HardwareSerial* s ; + byte rawRx[512]; + int lenRx = 0; + byte dataRx[512]; + int datalen = 0; + int SlaveID = 0x01; + byte txout[9] = {0,0,0,0,0,0,0,0,0}; + #define Coil_Register 0x01 + #define Discret_Register 0x02 + #define Holding_Register 0x03 + #define Input_Register 0x04 + #define Write_Holding_Register 0x06 + // vector txbuff; + // vector rxbuff; + +public: + + Modbus(); + Modbus(HardwareSerial &st); + + bool init(int mode, bool en_log = false); + void setTimeout(uint16_t timeout); + + + byte byteRead(int nb); + int blockRead(int index); + int coilRead(int address); //Return 1 byte = 8 bit coil + int coilRead(int id, int address); + int discreteInputRead(int address); + int discreteInputRead(int id, int address); + long holdingRegisterRead(int address); + long holdingRegisterRead(int id, int address, int block); + long inputRegisterRead(int address); + long inputRegisterRead(int id, int address, int block); + + int coilWrite(int address, uint8_t value); + int coilWrite(int id, int address, uint8_t value); + int holdingRegisterWrite(int address, uint16_t value); + int holdingRegisterWrite(int id, int address, uint16_t value); + void RxRaw(byte *raw, uint8_t &rlen); + void TxRaw(byte *raw, uint8_t &rlen); + //Read multiple coils, discrete inputs, holding registers, or input register values. + //int requestFrom(int type, int address, int nb, byte *ret,int len); + int requestFrom(int slaveId, int type, int address,int nb); + // ~Modbus(); + + + // Read Coil Register 0x01 + int ReadCoilReg(int add); + int ReadCoilReg(int slaveId, int add); + int ReadCoilReg(int slaveId, int add, int nbit); + + // Read Discret Register 0x02 + int ReadDiscretReg(int add); + int ReadDiscretReg(int slaveId, int add); + int ReadDiscretReg(int slaveId, int add, int nbit); + + // Read Holding Register 0x03 + int ReadHoldingReg(int add); + int ReadHoldingReg(int slaveId, int add); + int ReadHoldingReg(int slaveId, int add, int nbyte); + + // Read Input Register 0x04 + int ReadInputReg(int add); + int ReadInputReg(int slaveId, int add); + int ReadInputReg(int slaveId, int add, int nbyte); + + + int8_t uint8(int add); + uint16_t uint16(int add); + uint32_t uint32(int add, bool byteHL = true); + + + + int CheckCRC(byte *buf, int len); +}; + +// #else +// #error "Log not defined" +// #endif + +#endif diff --git a/Arduino/ModbusTest/ModbusTest.ino b/Arduino/ModbusTest/ModbusTest.ino new file mode 100644 index 00000000..a183e8fe --- /dev/null +++ b/Arduino/ModbusTest/ModbusTest.ino @@ -0,0 +1,137 @@ +#include +#include "Modbus.h" + +// define modbus stuff +#define TXPIN 27 //17 +#define RXPIN 26 // 16 +#define MODE 5 +Modbus modbus(Serial1); + + + + +// servo states register addresses +#define reg_add_position_given_p 0x0001 // checked +#define reg_add_position_feedback_p 0x0002 // checked +#define reg_add_position_error_p 0x0003 // checked +#define reg_add_command_position_given_p 0x0004 // checked +#define reg_add_position_relative_error_p 0x0005 // checked +#define reg_add_velocity_given_rpm 0x0040 // checked +#define reg_add_velocity_feedback_rpm 0x0041 // checked +#define reg_add_velocity_error_rpm 0x0042 // checked +#define reg_add_velocity_feedback_no_filt_rpm 0x0048 // checked +#define reg_add_position_command_velocity_rpm 0x0049 // checked +#define reg_add_velocity_current_given_percent 0x0080 // checked +#define reg_add_velocity_current_feedback_percent 0x0081 // checked + +#define ref_cyclic_read_0 0x01F3 +#define ref_cyclic_read_1 0x01F4 +#define ref_cyclic_read_2 0x01F5 +#define ref_cyclic_read_3 0x01F6 + +// servo parameter addresses +#define pr_0_00 0x0000 // reserved parameter +#define pr_1_00 0x0000 + 25 // 1st position gain +#define pr_2_00 pr_1_00 + 40 // adaptive filter mode setup +#define pr_3_00 pr_2_00 + 30 // velocity control +#define pr_4_00 pr_3_00 + 30 // velocity torque control +#define pr_5_00 pr_4_00 + 50 // extension settings +#define pr_6_00 pr_5_00 + 40 // special settings + + +#define slaveId 63 + + +void setup() { + Serial.begin(115200); // Serial to PC + Serial1.begin(38400, SERIAL_8N1, RXPIN, TXPIN, true); // Modbus serial + modbus.init(MODE); + + + // The iSV57 has four registers (0x0191, 0x0192, 0x0193, 0x0194) in which we can write, which values we want to obtain cyclicly + // These registers can be obtained by sending e.g. the command: 0x63, 0x03, 0x0191, target_sate, CRC + // tell the modbus slave, which registers will be read cyclicly + delay(1000); + modbus.holdingRegisterWrite(slaveId, 0x0191, reg_add_position_given_p); + delay(50); + modbus.holdingRegisterWrite(slaveId, 0x0192, reg_add_position_error_p); + delay(50); + modbus.holdingRegisterWrite(slaveId, 0x0193, reg_add_velocity_current_feedback_percent); + delay(50); + modbus.holdingRegisterWrite(slaveId, 0x0194, reg_add_velocity_current_given_percent); + delay(50); + + + // servo config update + modbus.holdingRegisterWrite(slaveId, pr_0_00+2, 0); // deactivate auto gain + delay(50); + modbus.holdingRegisterWrite(slaveId, pr_0_00+3, 10); // machine stiffness + delay(50); + modbus.holdingRegisterWrite(slaveId, pr_0_00+4, 80); // ratio of inertia + delay(50); + modbus.holdingRegisterWrite(slaveId, pr_0_00+8, 1600); // microsteps + delay(50); + modbus.holdingRegisterWrite(slaveId, pr_0_00+14, 500); // position deviation setup + delay(50); + modbus.holdingRegisterWrite(slaveId, pr_1_00+0, 20); // 1st position gain + delay(50); + modbus.holdingRegisterWrite(slaveId, pr_1_00+1, 20); // 1st velocity loop gain + delay(50); + modbus.holdingRegisterWrite(slaveId, pr_1_00+15, 0); // control switching mode + delay(50); + + + + // store the settings to servos NVM + if (0) + { + modbus.holdingRegisterWrite(slaveId, 0x019A, 0x5555); // store the settings to servos NVM + delay(2000); + } + + + +} + + +byte raw[200]; +uint8_t len; + + + + +void loop() { + delay(10); + + + int16_t regArray[4]; + + // read the four registers + for (uint8_t regIdx = 0; regIdx < 4; regIdx++) + { + if(modbus.requestFrom(slaveId, 0x03, ref_cyclic_read_0 + regIdx, 1) > 0) + { + modbus.RxRaw(raw, len); + regArray[regIdx] = modbus.uint16(0); + } + delay(10); + } + + + + + Serial.print("Pos_given:"); + Serial.print(regArray[0]); + + Serial.print(",Pos_error:"); + Serial.print(regArray[1]); + + Serial.print(",Cur_given:"); + Serial.print(regArray[2]); + + Serial.print(",Cur_fb:"); + Serial.print(regArray[3]); + + Serial.println(" "); + +}