I need help with ModBus TCP-IP to read and write to the same Register using FC3 and FC16

[rocketman_2205]

Hello everyone, I want to use Arduino Mega 2560 as modbus slave to simulate a motor driver. I am using modbus Poll as a master. Now, the Motor driver that I am using requires its control word to be written in holding register 40005 and the speed in 40006. The output from motor like position and actual speed are again stored in 40005 and 40006 respectively. This is where I am having a Problem. For writing to the registers driver uses FC16 and for reading it uses FC3. So from my understanding, even if the address of the registers are same, they are actually different addresses due to different function code. How do I go about with incorporating this in an arduino code? I need to read and write to registers with same addresses which are in fact different to each other. Right now if I write to a certain register and then read some input in the same register, the motor stops running as the control word in that register has changed.

#include <Ethernet.h>
#include <ArduinoModbus.h>

bool _1s;
unsigned long TimeAct, TimePrev, HoldingResult, InputResult, Pulse, i, StartingAddr, Start;
long Cmd;

EthernetServer EthServer(502);
ModbusTCPServer modbusTCPServer;

void setup() {
  // Ethernet Settings
  byte mac[] = { 0x4E, 0xA0, 0xBE, 0x3F, 0xFE, 0x0F };  // Define MAc address
  byte ip[] = { 192, 168, 10, 50 };                      // Define IP address
  byte subnet[] = { 255, 255, 255, 0 };                 // Define SubNEt mask

  // initialize the ethernet device
  Ethernet.begin(mac, ip, subnet);                      // Assign MAC, IP, and subnet mask
  Serial.begin(9600);
  EthServer.begin();          // start listening for clients
  modbusTCPServer.begin();    // start listening for clients

  // Define Holding register:
  HoldingResult = modbusTCPServer.configureHoldingRegisters(0, 100);
  InputResult = modbusTCPServer.configureInputRegisters(0, 100);


  Serial.print("Holding Reg init result =");
  Serial.print(HoldingResult);
  Serial.print("\n");

  Serial.print("Input Reg init result =");
  Serial.print(InputResult);
  Serial.print("\n");

  Serial.print("Modbus server address=");
  Serial.println(Ethernet.localIP());
  Serial.print("\n");
}

void loop() {
  // Modbus server accept incoming connections
  EthernetClient client = EthServer.available();

  //Change Below command if issue running for HR 40002
  Start = modbusTCPServer.holdingRegisterRead(0x01);

  if (client.connected()) {
    modbusTCPServer.accept(client);
    // poll for Modbus TCP requests, while client connected
    modbusTCPServer.poll();
    // Serial.print("poll");
  }

  //---------------------------------------------------------------------------------
  // TIME  clock 1s
  TimeAct = millis();      // Millis value between  0 and  655535

  _1s = false;
  if (  ( (TimeAct > TimePrev) and (TimeAct - TimePrev) > 1000) or ((TimeAct < TimePrev) and (65535 - TimePrev + TimeAct) > 1000 ))
  {
    _1s = true;
    TimePrev = TimeAct;
  }
  //---------------------------------------------------------------------------------
  // Pulse 
  if (_1s) {
    if (Start == 1){
      Pulse++;
      if (Pulse > 255) {
        Pulse = 0;
    }
    Serial.print("Pulse=");
    Serial.println(Pulse);
    Serial.print("\n");
    Serial.print(Start);
    Serial.print("\n");
         for (i=0; i<20; i++)
      {Serial.print("-");
      }
      Serial.print("\n");
  }
    else if (Start == 2) {
      Pulse = 0;
      Serial.print(Start);
    }
  }

  //---------------------------------------------------------------------------------
  // Modbus server  :

  // holding resgiter 40001: Pulse (FC3)
  modbusTCPServer.holdingRegisterWrite(0x00, Pulse);

if (_1s) {
    Serial.print("cmd=");
    Serial.println(Cmd);
    Serial.print("\n");
  }

}