esphome:
  name: mt4v2
  friendly_name: mt4v2


# ============================================================
# ESP32-S3
# ============================================================

esp32:
  board: esp32-s3-devkitc-1
  framework:
    type: arduino


# ============================================================
# Logger
# ============================================================

logger:
  hardware_uart: USB_SERIAL_JTAG


# ============================================================
# Home Assistant API
# ============================================================

api:


# ============================================================
# Ethernet - W5500
# ============================================================

ethernet:
  type: W5500
  clk_pin: GPIO1
  mosi_pin: GPIO2
  miso_pin: GPIO41
  cs_pin: GPIO42
  interrupt_pin: GPIO43
  reset_pin: GPIO44


# ============================================================
# I2C
# ============================================================

i2c:
  - id: bus_a
    sda: 8
    scl: 18
    scan: true
    frequency: 400kHz


# ============================================================
# PCF8574 - Digital Inputs
# ============================================================

pcf8574:
  - id: pcf8574_hub_in_1
    i2c_id: bus_a
    address: 0x24


# ============================================================
# UART
# ============================================================

uart:

  # RS485 UART
  - id: uart_1
    baud_rate: 9600
    tx_pin: 39
    rx_pin: 38
    debug:
      direction: BOTH
      dummy_receiver: true
      after:
        timeout: 10ms

  # ARM CPU Motor Control UART
  # ESP32-S3 TX GPIO16 -> ARM RX
  # ESP32-S3 RX GPIO15 <- ARM TX
  # MODBUS RTU 115200 / 8N1
  - id: uart_motor
    baud_rate: 115200
    tx_pin: 16
    rx_pin: 15
    data_bits: 8
    parity: NONE
    stop_bits: 1


# ============================================================
# Global Motor Speed Variables
#
# Positive = Forward
# Negative = Reverse
# 0        = Stop
# ============================================================

globals:

  - id: motor1_speed
    type: int
    restore_value: no
    initial_value: '100'

  - id: motor2_speed
    type: int
    restore_value: no
    initial_value: '100'

  - id: motor3_speed
    type: int
    restore_value: no
    initial_value: '100'

  - id: motor4_speed
    type: int
    restore_value: no
    initial_value: '100'


# ============================================================
# Motor MODBUS Scripts
# ============================================================

script:

  # ----------------------------------------------------------
  # Motor Master Enable
  # Coil 503 = ON
  # ----------------------------------------------------------

  - id: motor_enable
    then:
      - lambda: |-
          uint8_t data[6] = {
            0x01, 0x05,
            0x01, 0xF7,
            0xFF, 0x00
          };

          uint16_t crc = 0xFFFF;

          for (int p = 0; p < 6; p++) {
            crc ^= data[p];

            for (int i = 0; i < 8; i++) {
              crc = (crc & 1)
                    ? (crc >> 1) ^ 0xA001
                    : crc >> 1;
            }
          }

          id(uart_motor).write_array(data, 6);

          uint8_t crc_data[2] = {
            (uint8_t)(crc & 0xFF),
            (uint8_t)(crc >> 8)
          };

          id(uart_motor).write_array(crc_data, 2);


  # ----------------------------------------------------------
  # Motor 1 - Register 2000
  # ----------------------------------------------------------

  - id: motor1_write
    mode: restart
    then:
      - lambda: |-
          int16_t speed = id(motor1_speed);

          uint8_t data[6] = {
            0x01, 0x06,
            0x07, 0xD0,
            (uint8_t)(speed >> 8),
            (uint8_t)speed
          };

          uint16_t crc = 0xFFFF;

          for (int p = 0; p < 6; p++) {
            crc ^= data[p];

            for (int i = 0; i < 8; i++) {
              crc = (crc & 1)
                    ? (crc >> 1) ^ 0xA001
                    : crc >> 1;
            }
          }

          id(uart_motor).write_array(data, 6);

          uint8_t crc_data[2] = {
            (uint8_t)(crc & 0xFF),
            (uint8_t)(crc >> 8)
          };

          id(uart_motor).write_array(crc_data, 2);


  # ----------------------------------------------------------
  # Motor 2 - Register 2001
  # ----------------------------------------------------------

  - id: motor2_write
    mode: restart
    then:
      - lambda: |-
          int16_t speed = id(motor2_speed);

          uint8_t data[6] = {
            0x01, 0x06,
            0x07, 0xD1,
            (uint8_t)(speed >> 8),
            (uint8_t)speed
          };

          uint16_t crc = 0xFFFF;

          for (int p = 0; p < 6; p++) {
            crc ^= data[p];

            for (int i = 0; i < 8; i++) {
              crc = (crc & 1)
                    ? (crc >> 1) ^ 0xA001
                    : crc >> 1;
            }
          }

          id(uart_motor).write_array(data, 6);

          uint8_t crc_data[2] = {
            (uint8_t)(crc & 0xFF),
            (uint8_t)(crc >> 8)
          };

          id(uart_motor).write_array(crc_data, 2);


  # ----------------------------------------------------------
  # Motor 3 - Register 2002
  # ----------------------------------------------------------

  - id: motor3_write
    mode: restart
    then:
      - lambda: |-
          int16_t speed = id(motor3_speed);

          uint8_t data[6] = {
            0x01, 0x06,
            0x07, 0xD2,
            (uint8_t)(speed >> 8),
            (uint8_t)speed
          };

          uint16_t crc = 0xFFFF;

          for (int p = 0; p < 6; p++) {
            crc ^= data[p];

            for (int i = 0; i < 8; i++) {
              crc = (crc & 1)
                    ? (crc >> 1) ^ 0xA001
                    : crc >> 1;
            }
          }

          id(uart_motor).write_array(data, 6);

          uint8_t crc_data[2] = {
            (uint8_t)(crc & 0xFF),
            (uint8_t)(crc >> 8)
          };

          id(uart_motor).write_array(crc_data, 2);


  # ----------------------------------------------------------
  # Motor 4 - Register 2003
  # ----------------------------------------------------------

  - id: motor4_write
    mode: restart
    then:
      - lambda: |-
          int16_t speed = id(motor4_speed);

          uint8_t data[6] = {
            0x01, 0x06,
            0x07, 0xD3,
            (uint8_t)(speed >> 8),
            (uint8_t)speed
          };

          uint16_t crc = 0xFFFF;

          for (int p = 0; p < 6; p++) {
            crc ^= data[p];

            for (int i = 0; i < 8; i++) {
              crc = (crc & 1)
                    ? (crc >> 1) ^ 0xA001
                    : crc >> 1;
            }
          }

          id(uart_motor).write_array(data, 6);

          uint8_t crc_data[2] = {
            (uint8_t)(crc & 0xFF),
            (uint8_t)(crc >> 8)
          };

          id(uart_motor).write_array(crc_data, 2);


# ============================================================
# Motor Speed Sliders
#
# 0   = Stop
# 100 = Full Speed
#
# Slider changes motor speed immediately.
# Current direction is retained.
# ============================================================

number:

  - platform: template
    name: "Motor 1 Speed"
    id: motor1_speed_slider
    min_value: 0
    max_value: 100
    step: 1
    initial_value: 100
    optimistic: true
    mode: slider
    set_action:
      - lambda: |-
          if (id(motor1_speed) < 0)
            id(motor1_speed) = -(int)x;
          else
            id(motor1_speed) = (int)x;
      - script.execute: motor1_write


  - platform: template
    name: "Motor 2 Speed"
    id: motor2_speed_slider
    min_value: 0
    max_value: 100
    step: 1
    initial_value: 100
    optimistic: true
    mode: slider
    set_action:
      - lambda: |-
          if (id(motor2_speed) < 0)
            id(motor2_speed) = -(int)x;
          else
            id(motor2_speed) = (int)x;
      - script.execute: motor2_write


  - platform: template
    name: "Motor 3 Speed"
    id: motor3_speed_slider
    min_value: 0
    max_value: 100
    step: 1
    initial_value: 100
    optimistic: true
    mode: slider
    set_action:
      - lambda: |-
          if (id(motor3_speed) < 0)
            id(motor3_speed) = -(int)x;
          else
            id(motor3_speed) = (int)x;
      - script.execute: motor3_write


  - platform: template
    name: "Motor 4 Speed"
    id: motor4_speed_slider
    min_value: 0
    max_value: 100
    step: 1
    initial_value: 100
    optimistic: true
    mode: slider
    set_action:
      - lambda: |-
          if (id(motor4_speed) < 0)
            id(motor4_speed) = -(int)x;
          else
            id(motor4_speed) = (int)x;
      - script.execute: motor4_write


# ============================================================
# Motor Control Buttons
# ============================================================

button:

  # Motor 1
  - platform: template
    name: "Motor 1 Forward"
    on_press:
      - script.execute: motor_enable
      - delay: 50ms
      - lambda: |-
          id(motor1_speed) =
            abs((int)id(motor1_speed_slider).state);
      - script.execute: motor1_write

  - platform: template
    name: "Motor 1 Reverse"
    on_press:
      - script.execute: motor_enable
      - delay: 50ms
      - lambda: |-
          id(motor1_speed) =
            -abs((int)id(motor1_speed_slider).state);
      - script.execute: motor1_write

  - platform: template
    name: "Motor 1 Stop"
    on_press:
      - lambda: |-
          id(motor1_speed) = 0;
      - script.execute: motor1_write


  # Motor 2
  - platform: template
    name: "Motor 2 Forward"
    on_press:
      - script.execute: motor_enable
      - delay: 50ms
      - lambda: |-
          id(motor2_speed) =
            abs((int)id(motor2_speed_slider).state);
      - script.execute: motor2_write

  - platform: template
    name: "Motor 2 Reverse"
    on_press:
      - script.execute: motor_enable
      - delay: 50ms
      - lambda: |-
          id(motor2_speed) =
            -abs((int)id(motor2_speed_slider).state);
      - script.execute: motor2_write

  - platform: template
    name: "Motor 2 Stop"
    on_press:
      - lambda: |-
          id(motor2_speed) = 0;
      - script.execute: motor2_write


  # Motor 3
  - platform: template
    name: "Motor 3 Forward"
    on_press:
      - script.execute: motor_enable
      - delay: 50ms
      - lambda: |-
          id(motor3_speed) =
            abs((int)id(motor3_speed_slider).state);
      - script.execute: motor3_write

  - platform: template
    name: "Motor 3 Reverse"
    on_press:
      - script.execute: motor_enable
      - delay: 50ms
      - lambda: |-
          id(motor3_speed) =
            -abs((int)id(motor3_speed_slider).state);
      - script.execute: motor3_write

  - platform: template
    name: "Motor 3 Stop"
    on_press:
      - lambda: |-
          id(motor3_speed) = 0;
      - script.execute: motor3_write


  # Motor 4
  - platform: template
    name: "Motor 4 Forward"
    on_press:
      - script.execute: motor_enable
      - delay: 50ms
      - lambda: |-
          id(motor4_speed) =
            abs((int)id(motor4_speed_slider).state);
      - script.execute: motor4_write

  - platform: template
    name: "Motor 4 Reverse"
    on_press:
      - script.execute: motor_enable
      - delay: 50ms
      - lambda: |-
          id(motor4_speed) =
            -abs((int)id(motor4_speed_slider).state);
      - script.execute: motor4_write

  - platform: template
    name: "Motor 4 Stop"
    on_press:
      - lambda: |-
          id(motor4_speed) = 0;
      - script.execute: motor4_write


# ============================================================
# RS485 Test Switch
# ============================================================

switch:

  - platform: uart
    uart_id: uart_1
    name: "RS485 Button"
    data: [0x11, 0x22, 0x33, 0x44, 0x55]


# ============================================================
# Digital Inputs
# ============================================================

binary_sensor:

  - platform: gpio
    name: "mt4v2-input01"
    id: mt4v2_input01
    pin:
      pcf8574: pcf8574_hub_in_1
      number: 0
      mode: INPUT
      inverted: true

  - platform: gpio
    name: "mt4v2-input02"
    id: mt4v2_input02
    pin:
      pcf8574: pcf8574_hub_in_1
      number: 1
      mode: INPUT
      inverted: true

  - platform: gpio
    name: "mt4v2-input03"
    id: mt4v2_input03
    pin:
      pcf8574: pcf8574_hub_in_1
      number: 2
      mode: INPUT
      inverted: true

  - platform: gpio
    name: "mt4v2-input04"
    id: mt4v2_input04
    pin:
      pcf8574: pcf8574_hub_in_1
      number: 3
      mode: INPUT
      inverted: true

  - platform: gpio
    name: "mt4v2-input05"
    id: mt4v2_input05
    pin:
      pcf8574: pcf8574_hub_in_1
      number: 4
      mode: INPUT
      inverted: true

  - platform: gpio
    name: "mt4v2-input06"
    id: mt4v2_input06
    pin:
      pcf8574: pcf8574_hub_in_1
      number: 5
      mode: INPUT
      inverted: true

  - platform: gpio
    name: "mt4v2-input07"
    id: mt4v2_input07
    pin:
      pcf8574: pcf8574_hub_in_1
      number: 6
      mode: INPUT
      inverted: true

  - platform: gpio
    name: "mt4v2-input08"
    id: mt4v2_input08
    pin:
      pcf8574: pcf8574_hub_in_1
      number: 7
      mode: INPUT
      inverted: true


  # GPIO inputs with pull-up resistors on PCB

  - platform: gpio
    name: "mt4v2-W1-io47"
    pin:
      number: 47
      inverted: true

  - platform: gpio
    name: "mt4v2-W1-io48"
    pin:
      number: 48
      inverted: true

  - platform: gpio
    name: "mt4v2-W1-io17"
    pin:
      number: 17
      inverted: true

  - platform: gpio
    name: "mt4v2-W1-io40"
    pin:
      number: 40
      inverted: true


  # ==========================================================
  # DRV8262 Fault Status
  #
  # Register 2020 = DRV8262 #1
  # Register 2021 = DRV8262 #2
  #
  # OFF = Normal
  # ON  = Problem
  # ==========================================================

  - platform: template
    name: "DRV8262-1 Fault"
    id: drv8262_1_fault
    device_class: problem

  - platform: template
    name: "DRV8262-2 Fault"
    id: drv8262_2_fault
    device_class: problem


# ============================================================
# ADS1115
# ============================================================

ads1115:
  - address: 0x48


# ============================================================
# Sensors
# ============================================================

sensor:

  # ADS1115 Channel 1
  - platform: ads1115
    multiplexer: A0_GND
    gain: 6.144
    resolution: 16_BITS
    name: "ADS1115 Channel A0-GND"
    update_interval: 5s

  # ADS1115 Channel 2
  - platform: ads1115
    multiplexer: A1_GND
    gain: 6.144
    name: "ADS1115 Channel A1-GND"
    update_interval: 5s

  # ADS1115 Channel 3
  - platform: ads1115
    multiplexer: A2_GND
    gain: 6.144
    name: "ADS1115 Channel A2-GND"
    update_interval: 5s

  # ADS1115 Channel 4
  - platform: ads1115
    multiplexer: A3_GND
    gain: 6.144
    name: "ADS1115 Channel A3-GND"
    update_interval: 5s


  # ==========================================================
  # Motor Current
  #
  # ARM MODBUS Registers:
  # 2010 = Motor 1 Current
  # 2011 = Motor 2 Current
  # 2012 = Motor 3 Current
  # 2013 = Motor 4 Current
  #
  # Values are displayed as raw register values.
  # ==========================================================

  - platform: template
    name: "Motor 1 Current"
    id: motor1_current
    accuracy_decimals: 0
    update_interval: never

  - platform: template
    name: "Motor 2 Current"
    id: motor2_current
    accuracy_decimals: 0
    update_interval: never

  - platform: template
    name: "Motor 3 Current"
    id: motor3_current
    accuracy_decimals: 0
    update_interval: never

  - platform: template
    name: "Motor 4 Current"
    id: motor4_current
    accuracy_decimals: 0
    update_interval: never


# ============================================================
# MODBUS Status Polling
#
# Every 2 seconds:
#
# Read 2010-2013 -> Motor Current
# Read 2020-2021 -> DRV8262 Fault
# ============================================================

interval:

  - interval: 2s
    then:

      # ======================================================
      # Read Motor Current
      #
      # FC03
      # Start Register = 2010
      # Quantity       = 4
      # ======================================================

      - lambda: |-

          // Clear old UART receive data
          while (id(uart_motor).available()) {
            uint8_t c;
            id(uart_motor).read_byte(&c);
          }

          // MODBUS request:
          // Slave 1
          // Function 03
          // Start address 2010 = 0x07DA
          // Read 4 registers

          uint8_t data[6] = {
            0x01, 0x03,
            0x07, 0xDA,
            0x00, 0x04
          };

          uint16_t crc = 0xFFFF;

          // Calculate MODBUS RTU CRC16
          for (int p = 0; p < 6; p++) {
            crc ^= data[p];

            for (int i = 0; i < 8; i++) {
              crc = (crc & 1)
                    ? (crc >> 1) ^ 0xA001
                    : crc >> 1;
            }
          }

          // Send MODBUS request
          id(uart_motor).write_array(data, 6);

          // Send CRC
          uint8_t crc_data[2] = {
            (uint8_t)(crc & 0xFF),
            (uint8_t)(crc >> 8)
          };

          id(uart_motor).write_array(crc_data, 2);


      # Wait for ARM CPU response
      - delay: 50ms


      # ======================================================
      # Process Motor Current Response
      # ======================================================

      - lambda: |-

          uint8_t rx[32];
          int len = 0;

          // Read UART response
          while (id(uart_motor).available() && len < 32) {
            id(uart_motor).read_byte(&rx[len]);
            len++;
          }

          // Expected response:
          //
          // 01 03 08
          // M1_H M1_L
          // M2_H M2_L
          // M3_H M3_L
          // M4_H M4_L
          // CRC_L CRC_H
          //
          // Total = 13 bytes

          if (len >= 13 &&
              rx[0] == 0x01 &&
              rx[1] == 0x03 &&
              rx[2] == 0x08) {

            uint16_t m1 =
              ((uint16_t)rx[3] << 8) | rx[4];

            uint16_t m2 =
              ((uint16_t)rx[5] << 8) | rx[6];

            uint16_t m3 =
              ((uint16_t)rx[7] << 8) | rx[8];

            uint16_t m4 =
              ((uint16_t)rx[9] << 8) | rx[10];

            // Publish values to ESPHome
            id(motor1_current).publish_state(m1);
            id(motor2_current).publish_state(m2);
            id(motor3_current).publish_state(m3);
            id(motor4_current).publish_state(m4);
          }


      # ======================================================
      # Read DRV8262 Fault Status
      #
      # FC03
      # Start Register = 2020
      # Quantity       = 2
      # ======================================================

      - lambda: |-

          // Clear UART receive buffer
          while (id(uart_motor).available()) {
            uint8_t c;
            id(uart_motor).read_byte(&c);
          }

          // MODBUS request:
          // Slave 1
          // Function 03
          // Start address 2020 = 0x07E4
          // Read 2 registers

          uint8_t data[6] = {
            0x01, 0x03,
            0x07, 0xE4,
            0x00, 0x02
          };

          uint16_t crc = 0xFFFF;

          // Calculate MODBUS RTU CRC16
          for (int p = 0; p < 6; p++) {
            crc ^= data[p];

            for (int i = 0; i < 8; i++) {
              crc = (crc & 1)
                    ? (crc >> 1) ^ 0xA001
                    : crc >> 1;
            }
          }

          // Send MODBUS request
          id(uart_motor).write_array(data, 6);

          // Send CRC
          uint8_t crc_data[2] = {
            (uint8_t)(crc & 0xFF),
            (uint8_t)(crc >> 8)
          };

          id(uart_motor).write_array(crc_data, 2);


      # Wait for ARM CPU response
      - delay: 50ms


      # ======================================================
      # Process DRV8262 Fault Response
      # ======================================================

      - lambda: |-

          uint8_t rx[16];
          int len = 0;

          // Read UART response
          while (id(uart_motor).available() && len < 16) {
            id(uart_motor).read_byte(&rx[len]);
            len++;
          }

          // Expected response:
          //
          // 01 03 04
          // Fault1_H Fault1_L
          // Fault2_H Fault2_L
          // CRC_L CRC_H
          //
          // Total = 9 bytes

          if (len >= 9 &&
              rx[0] == 0x01 &&
              rx[1] == 0x03 &&
              rx[2] == 0x04) {

            uint16_t fault1 =
              ((uint16_t)rx[3] << 8) | rx[4];

            uint16_t fault2 =
              ((uint16_t)rx[5] << 8) | rx[6];

            // 0 = Normal
            // Non-zero = Fault
            id(drv8262_1_fault).publish_state(
              fault1 != 0
            );

            id(drv8262_2_fault).publish_state(
              fault2 != 0
            );
          }


# ============================================================
# Web Server
# ============================================================

web_server:
  port: 80


# ============================================================
# OLED Font
# ============================================================

font:
  - file: "gfonts://Roboto"
    id: roboto
    size: 20


# ============================================================
# OLED Display
# ============================================================

display:

  - platform: ssd1306_i2c
    i2c_id: bus_a
    model: "SSD1306 128x64"
    address: 0x3C

    lambda: |-
      it.printf(
        0,
        0,
        id(roboto),
        "KinCony MT4v2"
      );