/************************************************************/
/************************************************************/
//Pin ESP32   MAX485
//16 (RX) ----> RO (Serial2)
//17 (TX) ----> DI (Serial2)
// 2      ----> RE
// 4       ----> DE
//5v      ----> VCC
//GND     ----> GND

//A(MAX485)  ----> A(USB)
//B(MAX485)  ----> B(USB)
/************************************************************/
/************************************************************/
#include <ModbusMaster.h>
#include <LiquidCrystal_I2C.h>      //SYSTEM PARAMETER  - ESP32 LCD Compatible Library (By: Robojax)
#include <PubSubClient.h>           //SYSTEM PARAMETER  - MQTT Library  (By: Adafruit)  
#include <Wire.h>                   // specify use of Wire.h library.
/************************************************************/
/************************************************************/
#define RXD2 (16)
#define TXD2 (17)

#define MAX485_DE      (2)
#define MAX485_RE_NEG  (4)

#define SWITCH_ON                 (12)                                //Pin D12  SWITCH ON
#define SWITCH_DIR                (14)                                //Pin D14  SWITCH DIRECTION     
#define SWITCH_RESET              (15)                                //Pin D15  SWITCH RESET     

#define BUTTON_START              digitalRead(SWITCH_ON)   == 1       // Set Input Pin
#define BUTTON_DIR                digitalRead(SWITCH_DIR)  == 1       // Set Input Pin
#define BUTTON_RESET              digitalRead(SWITCH_RESET)== 1       // Set Input Pin
#define Debounce                  (10)                                  // SW debounce
#define Delay_ms                  (1000)   

#define Slave_ID                  (1)
#define QUANTITY                  (50)

#define Address_3_FREQ_REF        (3)
#define Address_4_RUN_ENABLE      (4)
#define Address_5_CMD_FWD_REV     (5)
#define Address_6_CMD_START       (6)
#define Address_7_FAULT_ACK       (7)

#define Address_16_REF_FREQ       (16)

#define Address_19_P_GAIN         (19)
#define Address_20_I_GAIN         (20)
#define Address_21_D_GAIN         (21)

#define Address_23_Freq_Out       (23)
#define Address_24_Speed_Motor    (24)
#define Address_25_Current_Motor  (25)
#define Address_26_Torque_Motor   (26)
#define Address_27_Power_Motor    (27)
#define Address_28_ACTUAL_PWR     (28)

#define Address_29_TOTAL_KWH      (29)
#define Address_30_DC_BUS_VOLTS   (30)
#define Address_31_Freq_REFERENCE (31)
#define Address_32_RATED_PWR      (32)
#define Address_33_OUTPUT_VOLTS   (33)

#define Address_99_CMD_Command    (99)
#define Address_100_Freq_Set      (100)

#define Address_341_SPEED_RPM     (341)
#define Address_342_FREQ_OUTPUT   (342)
#define Address_343_OUTPUT_VOLTS  (343)  //Unit :  V; Scelling :1
#define Address_344_DC_BUS_VOLTS  (344)  //Unit :  V; Scelling :1
#define Address_345_CURRENT       (345)  //Unit :  A; Scelling :100
#define Address_346_TORQUE        (346)  //Unit : Nm; Scelling :100


#define VFD_Reset                 (1278)
#define Stop                      (1150)
#define Reverse_Run               (3199)
#define Forward_Run               (1151)
/************************************************************/
/************************************************************/
uint8_t i = 0;
unsigned long last_time = 0;

/*********Struct command Control *******/
volatile struct command{
  union{
    unsigned short WORD;
    struct {

        // MSB High Byte
        unsigned char     :8;

        
        // MSB Low Byte

        unsigned char ButtonDir    :1;
        unsigned char Button       :1; 
        unsigned char Rotate       :1; 
        unsigned char Enable       :1; 
        unsigned char Dir          :1;
        unsigned char Reset        :1;
        unsigned char Emer         :1;
        unsigned char Run          :1;
        }BIT;
    }CTRL;
}CMD;

volatile unsigned int  
event_start = 0,  
event_reset = 0,   
event_dir   = 0; 
 
volatile unsigned long 
lastime_0 = 0,
lastime_1 = 0,
lastime_2 = 0,
lastime_3 = 0;
/************************************************************/
/************************************************************/
// instantiate ModbusMaster object
ModbusMaster node;
LiquidCrystal_I2C lcd = LiquidCrystal_I2C(0x27, 20,4);
/************************************************************/
/************************************************************/
void preTransmission()
{
  digitalWrite(MAX485_RE_NEG, 1);
  digitalWrite(MAX485_DE, 1);
}
/************************************************************/
/************************************************************/
void postTransmission()
{
  digitalWrite(MAX485_RE_NEG, 0);
  digitalWrite(MAX485_DE, 0);
}
/************************************************************/
/************************************************************/
void setup()
{
  pinMode(SWITCH_ON,   INPUT);                              //Pin D12  SWITCH ON
  pinMode(SWITCH_DIR,  INPUT);                              //Pin D14  SWITCH OFF 
  pinMode(SWITCH_RESET,INPUT);                              //Pin D15  SWITCH RESET 

  delay(100);

  pinMode(MAX485_RE_NEG, OUTPUT);
  pinMode(MAX485_DE, OUTPUT);
  
  // Init in receive mode
  digitalWrite(MAX485_RE_NEG, 0);
  digitalWrite(MAX485_DE, 0);

  // Modbus communication runs at 115200 baud
  Serial.begin(115200);
  Serial2.begin(9600,SERIAL_8N1, RXD2, TXD2);
  
  // Modbus slave ID 1
  node.begin(Slave_ID, Serial2);
  // Callbacks allow us to configure the RS485 transceiver correctly
  node.preTransmission(preTransmission);
  node.postTransmission(postTransmission);
  
  lcd.init();                   // initial LCD on lib
  lcd.setBacklight(HIGH);
  
  lcd.setCursor(0,0);
  lcd.print("Inverter Control");
  
  lcd.setCursor(0,1);
  lcd.print("SINAMICS V20");
  delay(1500);
  
  lcd.clear();

  CMD.CTRL.BIT.Dir   = 0;         // Clear flag motor direction
  CMD.CTRL.BIT.Run   = 0;         // Clear flag Inverter Start : Stop
  CMD.CTRL.BIT.Reset = 0;         // Clear flag Reset Inverter
}
/************************************************************/
/************************************************************/
void loop()
{
  uint8_t result = 0;
  uint8_t respons = 0;
  uint16_t data[QUANTITY];

 if(millis()- last_time > Delay_ms)
 {
 /*
  i++;
  if (i > 100)
  {
    i = 0; 
  }
*/
  //node.writeSingleRegister(0x40100,Forward_Run);
  //node.setTransmitBuffer(0x40100, Forward_Run);
  //node.writeSingleRegister(0x40100,Reverse_Run);
  //node.setTransmitBuffer(0x40100, Reverse_Run);
  //node.writeSingleRegister(0x40100,Stop);
  //node.setTransmitBuffer(0x40100, Stop);
  //node.writeSingleRegister(0x40100,VFD_Reset);
  //node.setTransmitBuffer(0x40100, VFD_Reset);

  //node.setTransmitBuffer(0x40003, 888);
  //respons = node.writeMultipleRegisters(0x40000, 4);
  
  result = node.readHoldingRegisters(0x40000, QUANTITY);
  if (getResultMsg(&node, result)) 
  {
    Serial.println("Read data from Slave");
    for (int j = 0; j < QUANTITY; j++)
    {
      data[j] = node.getResponseBuffer(j);
      Serial.println(data[j]);
     }
   }

   InverterControl();                      // Inverter Control

   last_time = millis();

 }


  
  //ModbusDisplay();                        // Modbus RTU Display
  
  LCDdisplay();                           // LCD Display

}
/************************************************************/
/************************************************************/
bool getResultMsg(ModbusMaster *node, uint8_t result) 
{
  String tmpstr2 = "\r\n";
  switch (result) 
  {
  case node->ku8MBSuccess:
    return true;
    break;
  case node->ku8MBIllegalFunction:
    tmpstr2 += "Illegal Function";
    break;
  case node->ku8MBIllegalDataAddress:
    tmpstr2 += "Illegal Data Address";
    break;
  case node->ku8MBIllegalDataValue:
    tmpstr2 += "Illegal Data Value";
    break;
  case node->ku8MBSlaveDeviceFailure:
    tmpstr2 += "Slave Device Failure";
    break;
  case node->ku8MBInvalidSlaveID:
    tmpstr2 += "Invalid Slave ID";
    break;
  case node->ku8MBInvalidFunction:
    tmpstr2 += "Invalid Function";
    break;
  case node->ku8MBResponseTimedOut:
    tmpstr2 += "Response Timed Out";
    break;
  case node->ku8MBInvalidCRC:
    tmpstr2 += "Invalid CRC";
    break;
  default:
    tmpstr2 += "Unknown error: " + String(result);
    break;
  }
  Serial.println(tmpstr2);
  return false;
}
/************************************************************/
/************************************************************/
void InverterControl(void)
{
  ButtonControl();                        //  Button Control
  InverterStartStopControl();             //  Start : Stop Inverter
  InverterDirectionControl();             //  Motor Direction
  InverterResetControl();                 //  Seset button
}

/********************************************************************************
********************************************************************************/
void LCDdisplay(void)
{
  if(CMD.CTRL.BIT.Reset)
  {
    lcd.setCursor(0,0);
    lcd.print("RUN : " + String(CMD.CTRL.BIT.Run) + " : STOP");

    lcd.setCursor(0,1);
    lcd.print("DIR : " + String(CMD.CTRL.BIT.Dir) + " : CW");
  }
  else
  {
    lcd.setCursor(0,0);
  
    if (CMD.CTRL.BIT.Run)
    {
      lcd.print("RUN : " + String(CMD.CTRL.BIT.Run) + " : START");
    }
    else
    {
      lcd.print("RUN : " + String(CMD.CTRL.BIT.Run) + " : STOP ");
    }
    
    lcd.setCursor(0,1);
    
    if (CMD.CTRL.BIT.Dir)
    {
      lcd.print("DIR : " + String(CMD.CTRL.BIT.Dir) + " : CCW");
    }
    else
    {
      lcd.print("DIR : " + String(CMD.CTRL.BIT.Dir) + " : CW ");
    }
  }
}
/********************************************************************************
********************************************************************************/
void ButtonControl (void)
{
  //node.writeSingleRegister(0x40100,Forward_Run);
  //node.setTransmitBuffer(0x40100, Forward_Run);
  //node.writeSingleRegister(0x40100,Reverse_Run);
  //node.setTransmitBuffer(0x40100, Reverse_Run);
  //node.writeSingleRegister(0x40100,Stop);
  //node.setTransmitBuffer(0x40100, Stop);
  //node.writeSingleRegister(0x40100,VFD_Reset);
  //node.setTransmitBuffer(0x40100, VFD_Reset);

  if(CMD.CTRL.BIT.Run)
  {
    if(CMD.CTRL.BIT.Dir)
    {
      node.writeSingleRegister(0x40100,Forward_Run); // Call function write Motor direction CW
      node.setTransmitBuffer(0x40100, Forward_Run);
      Serial.println("Write data to Slave");
    }
    else
    {
      node.writeSingleRegister(0x40100,Reverse_Run);// Call function write Motor direction CCW
      node.setTransmitBuffer(0x40100, Reverse_Run);  
      Serial.println("Write data to Slave");
    }
  }
  else 
  {
    if(CMD.CTRL.BIT.Dir)
    {
      node.writeSingleRegister(0x40100,Stop);
      node.setTransmitBuffer(0x40100, Stop);             
    }
  }
}
/********************************************************************************
********************************************************************************/
void InverterResetControl(void)
{
  if(BUTTON_RESET)                          // Motor Direction
  {
     long now = millis();
     if((now - lastime_2) > Debounce)         //  Software debounce
     {
        lastime_2 = millis();;
        if(event_reset == 0)                // Press button 1st step command CCW 
        {
           CMD.CTRL.BIT.Reset = 1;          // Set CCW flag
           CMD.CTRL.BIT.Run   = 0;          // Clear flag Inverter Start : Stop
           CMD.CTRL.BIT.Dir   = 0;          // Set CW flag 
           event_reset = 1;                 // Next event command
           
           node.writeSingleRegister(0x40100,VFD_Reset);
           node.setTransmitBuffer(0x40100, VFD_Reset);
        }
     }
  }
  else
  {
     if(CMD.CTRL.BIT.Reset)                  // Check CCW flag
     {
       CMD.CTRL.BIT.Reset = 0;               // Set CW flag 
       event_reset = 0;                      // Clear event command
     }
  }
}
/********************************************************************************
********************************************************************************/
void InverterDirectionControl(void)
{
  if(BUTTON_DIR)                                  // Motor Direction
  {
     long now = millis();
     if((now - lastime_1) > Debounce)             //  Software debounce
     {
        lastime_1 = millis();;
        if(event_dir == 0)                        // Press button 1st step command CCW 
        {
           CMD.CTRL.BIT.Dir = 1;                  // Set CCW flag
           event_dir = 1;                         // Next event command
        }
     }
  }
  else
  {
     if(CMD.CTRL.BIT.Dir)                         // Check CCW flag
     {
       CMD.CTRL.BIT.Dir = 0;                      // Set CW flag 
       event_dir = 0;                             // Clear event command
     }
  }
}
/********************************************************************************
********************************************************************************/
void InverterStartStopControl(void)
{
   if(BUTTON_START)                               //  Start : Stop Inverter
   {
      long now = millis();                        //  Current time            
      if((now - lastime_0) > Debounce)            //  Software debounce
      {
        lastime_0 = millis();;                    // Clear count time
          
        if(event_start == 0)                      // Press button 1st step command start 
        {
           CMD.CTRL.BIT.Run = 1;                  // Set start flag
           event_start = 1;                       // Next event command
        }
        else if(event_start == 2)                 // Press button 3th step command stop 
        {
           CMD.CTRL.BIT.Run = 0;                  // Set stop flag
           event_start = 3;                       // Next event command
        }
      }
    }
    else
    {
      if(event_start == 1)                         // Unpress button 2nd step command hold for press stop  
      {
        event_start = 2;                           // Next event command                    
      }
      else if(event_start == 3)                    // Unpress button 3th step command 
      {
        event_start = 0;                           // Clear event command 
      }
    }
}
/********************************************************************************
********************************************************************************/

void ModbusDisplay(void)
{
  /*
  Modbus_Read_Holding_Reg(Slave_ID,Address_100_Freq_Set);                // Read Frequency Setting
  Modbus_Read_Holding_Reg(Slave_ID,Address_99_CMD_Command);              // Read CMD Command

  Modbus_Read_Holding_Reg(Slave_ID,Address_23_Freq_Out);                 // Read Frequency Output 
  Modbus_Read_Holding_Reg(Slave_ID,Address_24_Speed_Motor);              // Read Speed Motor
  Modbus_Read_Holding_Reg(Slave_ID,Address_25_Current_Motor);            // Read Current Motor
  Modbus_Read_Holding_Reg(Slave_ID,Address_26_Torque_Motor);             // Read Torque Motor 
  Modbus_Read_Holding_Reg(Slave_ID,Address_27_Power_Motor);              // Read Power Motor
  
  Modbus_Read_Holding_Reg(Slave_ID,Address_28_Power_Inv);                // Read Power Inverter
  Modbus_Read_Holding_Reg(Slave_ID,Address_29_DCBus_Inv);                // Read DC Bus Inverter 
  Modbus_Read_Holding_Reg(Slave_ID,Address_30_Freq_Inv);                 // Read Frequency Inverter 
  Modbus_Read_Holding_Reg(Slave_ID,Address_31_RatePWR_Inv);              // Read Rated Power Inverter 
  Modbus_Read_Holding_Reg(Slave_ID,Address_32_VolOut_Inv);               // Read Voltage Output Inverter 
  */
}