/*
Set direction and speed of AC Servo Motor 80AST-A1C04025 (Pn 1.0 kW - Tn 4.0 Nm - In 4.8 A) with Arduino Nano ATmega168 and RS485 TTL adapter
drive HL-T3D-L20A-RABF
Ennio Sesana 2024
*/

#include <SoftwareSerial.h>

const byte rxPin = 5;
const byte txPin = 6;

SoftwareSerial Serial1(rxPin, txPin);  // Serial channel for RS485 communication

int potPin = A0;
int switchPin = 7;
int potValue = 0;
bool direction = true;  // true for forward, false for reverse
//bool isServoEnabled = false;  // Variable to track servo status

// Function to calculate CRC16-Modbus checksum
uint16_t calculateCRC(byte* buf, int length) {
  uint16_t crc = 0xFFFF;  // Initialize CRC correctly
  for (int pos = 0; pos < length; pos++) {
    crc ^= (uint16_t)buf[pos];
    for (int i = 0; i < 8; i++) {
      if (crc & 0x0001) {
        crc = (crc >> 1) ^ 0xA001;
      } else {
        crc = crc >> 1;
      }
    }
  }
  return crc;
}

// Function to send Modbus request with CRC and print to serial
void sendModbusRequestWithCRC(byte* request, int length) {
  // Calculate checksum
  uint16_t crc = calculateCRC(request, length - 2);
  request[length - 2] = crc & 0xFF;         // LSB of CRC
  request[length - 1] = (crc >> 8) & 0xFF;  // MSB of CRC

  // Send the request and print to serial
  for (int i = 0; i < length; i++) {
    Serial1.write(request[i]);  // Send the byte
    //Serial.print(request[i] < 16 ? "0" : "");  // Add leading zero if byte is less than 16 for correct hexadecimal formatting
    //Serial.print(request[i], HEX);             // Print the byte in hexadecimal
    //Serial.print(" ");                         // Add a space between bytes for better readability
  }
  //Serial.println();  // Go to next line at the end of print
}

void setup() {

  Serial.begin(19200);
  Serial1.begin(19200);

  pinMode(potPin, INPUT);
  pinMode(switchPin, INPUT_PULLUP);

  // Read the maximum speed register
  readAndShowMaxSpeed();
 
  Serial.println("Ready to Start");
  delay(5000);

  // Reset the speed register
  byte resetSpeed[8] = { 0x01, 0x06, 0x00, 0x4C, 0x00, 0x00, 0x00, 0x00 };
  sendModbusRequestWithCRC(resetSpeed, sizeof(resetSpeed));

  delay(100);
}

unsigned long previousMillis = 0;
const long interval = 1000;  // Update interval in milliseconds

void loop() {
  int potValue = map(analogRead(potPin), 0, 1023, 0, 3000);
  int direction = digitalRead(switchPin);

  if (potValue > 10) {
    /*
    if (!isServoEnabled) {
      byte enableServo[3] = { 0x01, 0x42, 0x55 };
      sendModbusRequestWithCRC(enableServo, sizeof(enableServo));
      isServoEnabled = true;
    }
    */
    byte jogCommand[5];
    jogCommand[0] = 0x01;
    jogCommand[1] = 0x44;
    jogCommand[3] = 0x00;
    jogCommand[4] = 0x00;

    if (direction == HIGH) {
      jogCommand[2] = 0x01;  // JOG forward
    } else {
      jogCommand[2] = 0x02;  // JOG reverse
    }

    sendModbusRequestWithCRC(jogCommand, sizeof(jogCommand));

    delay(10);

    byte setSpeed[8] = { 0x01, 0x06, 0x00, 0x4C, highByte(potValue), lowByte(potValue), 0x00, 0x00 };

    sendModbusRequestWithCRC(setSpeed, sizeof(setSpeed));

  } else {
    byte disableJog[5] = { 0x01, 0x44, 0x00, 0x00, 0x00 };

    sendModbusRequestWithCRC(disableJog, sizeof(disableJog));

    /*
    byte disableServo[3] = { 0x01, 0x42, 0xAA };
    sendModbusRequestWithCRC(disableServo, sizeof(disableServo));
    isServoEnabled = false;
    */
  }

  delay(100);  // Update every 100 milliseconds

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    // Save current time as last update time
    previousMillis = currentMillis;
    readAndShowSpeed();
  }
}

void readAndShowSpeed() {

  // Tx: 01 04 10 00 00 01 35 0A
  // Modbus command to read speed (address 0x1000, 1 register)
  byte readSpeedCommand[8] = { 0x01, 0x04, 0x10, 0x00, 0x00, 0x01, 0x00, 0x00 };  // Address 0x1000, 1 register
  sendModbusRequestWithCRC(readSpeedCommand, sizeof(readSpeedCommand));           // Send Modbus command with included CRC

  // Wait for Modbus response
  delay(1);  // Adjust timing based on device response speed

  // Read Modbus response
  byte response[7];  // Array to store Modbus response
  int responseLength = 0;

  while (responseLength < 7) {
    if (Serial1.available() > 0) {
      byte currentByte = Serial1.read();

      // Add leading zero if hexadecimal value is single digit
      if (currentByte < 0x10) {
        Serial.print("0");
      }
      Serial.print(currentByte, HEX);
      Serial.print(" ");

      // Store only bytes identifying the response and speed
      if (responseLength < 7) {
        response[responseLength] = currentByte;
        responseLength++;
      }
    }
  }

  // Calculate CRC of the received response
  uint16_t receivedCRC = word(response[5], response[6]);                         // Bytes 5 and 6 represent the received CRC
  uint16_t calculatedCRC = calculateCRC(response, 5);                            // Calculate CRC on the first 5 bytes of the response
  uint16_t reversedCalculatedCRC = (calculatedCRC << 8) | (calculatedCRC >> 8);  // Reverse byte order in the calculated CRC

  // Check if received CRC matches calculated CRC
  if (receivedCRC == reversedCalculatedCRC) {
    Serial.println("Correct CRC: Modbus response is valid.");
    // Extract the speed from the register
    int speed = word(response[3], response[4]);  // Calculate current speed from bytes 3 and 4
    Serial.print("Current Speed: ");
    Serial.print(speed);
    Serial.println(" r/min");
  } else {
    Serial.println("CRC Error: Modbus response may be corrupted.");
  }
}

void readAndShowMaxSpeed() {

  // Tx: 01 03 00 CF 00 01 B4 35 request max Speed
  // Modbus command to read the maximum speed (address 0x004B, 1 register)
  byte readMaxSpeed[8] = { 0x01, 0x03, 0x00, 0xCF, 0x00, 0x01, 0x00, 0x00 };
  sendModbusRequestWithCRC(readMaxSpeed, sizeof(readMaxSpeed));  // Send Modbus command with included CRC

  // Wait for Modbus response
  delay(1);  // Adjust time based on device response speed

  // Read Modbus response
  byte response[7];  // Array to store Modbus response
  int responseLength = 0;

  while (responseLength < 7) {
    if (Serial1.available() > 0) {
      byte currentByte = Serial1.read();

      // Print byte in hexadecimal format (HEX)
      Serial.print(currentByte < 0x10 ? "0" : "");
      Serial.print(currentByte, HEX);
      Serial.print(" ");

      // Store response bytes
      if (responseLength < 7) {
        response[responseLength] = currentByte;
        responseLength++;
      }
    }
  }

  // Calculate CRC of the received response
  uint16_t receivedCRC = word(response[5], response[6]);                         // Received CRC
  uint16_t calculatedCRC = calculateCRC(response, 5);                            // Calculate CRC on the first 6 bytes of the response
  uint16_t reversedCalculatedCRC = (calculatedCRC << 8) | (calculatedCRC >> 8);  // Reverse byte order in the calculated CRC

  // Check if received CRC matches calculated CRC
  if (receivedCRC == reversedCalculatedCRC) {
    Serial.println("");
    Serial.println("Correct CRC: Modbus response is valid.");
    // Extract the maximum speed set from the register
    int maxSpeed = word(response[3], response[4]);  // Maximum speed set from bytes 3 and 4
    Serial.print("Max Set Speed: ");
    Serial.print(maxSpeed);
    Serial.println(" r/min");
  } else {
    Serial.println("");
    Serial.println("CRC Error: Modbus response may be corrupted.");
  }
}