/*
Copyright (C) 2022  Alpaca-zip

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <https://www.gnu.org/licenses/>.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

For the emergency stop to work correctly, you will need to:

1, Edit ModbusMaster.h

2, Line 252: static const uint16_t ku16MBResponseTimeout = 2000; => static const uint16_t ku16MBResponseTimeout = 100;
*/

#include <ModbusMaster.h>
#define MAX485_DE      3
#define MAX485_RE_NEG  2

ModbusMaster node; // instantiate ModbusMaster object

// set pin numbers:
const int estop_buttonPin = 7; // the number of the estop button pin
const int reset_buttonPin = 8; // the number of the reset button pin
const int ledPin =  13; // the number of the LED pin
int estop_buttonState = 0; // variable for reading the estop button status
int reset_buttonState = 0; // variable for reading the reset button status
bool estop_flag; // flag for emer_stop()
bool reset_flag; // flag for reset()

void preTransmission(){
  digitalWrite(MAX485_RE_NEG, 1);
  digitalWrite(MAX485_DE, 1);
}

void postTransmission(){
  digitalWrite(MAX485_RE_NEG, 0);
  digitalWrite(MAX485_DE, 0);
}

void emer_stop(){
  uint8_t estop_response;
  while(estop_flag){
    estop_response = node.writeSingleRegister(0x200E, 0x05); // 0x05: emergency stop
    if(estop_response == 0){
      digitalWrite(ledPin, HIGH); // turn LED on
      estop_flag = false;
      reset_flag = true;
    }
  }
}

void reset(){
  uint8_t reset_response;
  while(reset_flag){
    reset_response = node.writeSingleRegister(0x200E, 0x06); // 0x06: clear fault
    if(reset_response == 0) continue;
    reset_response = node.writeSingleRegister(0x200E, 0x07); // 0x07: stop
    if(reset_response == 0) continue;
    reset_response = node.writeSingleRegister(0x200E, 0x08); // 0x08: enable
    if(reset_response == 0){
      digitalWrite(ledPin, LOW); // turn LED off
      estop_flag = true;
      reset_flag = false;
    }
  }
}

void setup(){
  pinMode(ledPin, OUTPUT); // initialize the LED pin as an output:
  pinMode(estop_buttonPin, INPUT_PULLUP); // initialize the estop button pin as an input
  pinMode(reset_buttonPin, INPUT_PULLUP); // initialize the reset button pin as an input
  pinMode(MAX485_RE_NEG, OUTPUT); // initialize the MAX485_RE_NEG pin as an output
  pinMode(MAX485_DE, OUTPUT); // initialize the MAX485_DE pin as an output
  
  // Init in receive mode
  digitalWrite(MAX485_RE_NEG, 0);
  digitalWrite(MAX485_DE, 0);

  Serial.begin(115200); // modbus communication runs at 115200 baud
  node.begin(1, Serial); // modbus slave ID 1
  // callbacks allow us to configure the RS485 transceiver correctly
  node.preTransmission(preTransmission);
  node.postTransmission(postTransmission);

  estop_flag = true; // initialize the flag as a TRUE
  reset_flag = false; // initialize the flag as a FALSE
}
 
void loop(){
  estop_buttonState = digitalRead(estop_buttonPin); // read the state of the estop_buttonPin value
  reset_buttonState = digitalRead(reset_buttonPin); // read the state of the reset_buttonPin value

  if(estop_buttonState == LOW){ // estop button is pressed
    emer_stop();
  }else if(reset_buttonState == LOW){ // reset button is pressed
    reset();
  }
}