/**
* ============================================================
* BOJKE BL-30NZ-485 Laser Displacement Sensor
* Waveshare ESP32-S3-POE-ETH-8DI-8DO board
* ============================================================
*/
#include <Arduino.h>
#include <Wire.h>
// ── I2C pins (from I2C_Driver.h) ─────────────────────────────
#define I2C_SCL_PIN 41
#define I2C_SDA_PIN 42
// ── TCA9554 I2C expander (from WS_TCA9554PWR.h) ──────────────
#define TCA9554_ADDRESS 0x20
#define TCA9554_INPUT_REG 0x00
#define TCA9554_OUTPUT_REG 0x01
#define TCA9554_CONFIG_REG 0x03
// ── RS485 pins (from WS_GPIO.h) ──────────────────────────────
#define TXD1 17
#define RXD1 18
#define TXD1EN 21
// ── RS485 serial port ─────────────────────────────────────────
HardwareSerial rs485Serial(1);
// ── Modbus settings ───────────────────────────────────────────
#define MODBUS_BAUD 115200
#define MODBUS_SLAVE_ID 0x01
#define SENSOR_REG_ADDR 0x0000
#define SENSOR_REG_COUNT 0x0002
#define MODBUS_RESPONSE_TIMEOUT_MS 500
#define EXPECTED_RESPONSE_LEN 9
#define POLL_INTERVAL_MS 1000
// ─────────────────────────────────────────────────────────────
// TCA9554 helpers
// ─────────────────────────────────────────────────────────────
void TCA9554_WriteReg(uint8_t reg, uint8_t data) {
Wire.beginTransmission(TCA9554_ADDRESS);
Wire.write(reg);
Wire.write(data);
Wire.endTransmission();
}
void TCA9554_Init(uint8_t pinMode, uint8_t pinState) {
TCA9554_WriteReg(TCA9554_OUTPUT_REG, pinState);
TCA9554_WriteReg(TCA9554_CONFIG_REG, pinMode);
}
// ─────────────────────────────────────────────────────────────
// CRC-16/IBM
// ─────────────────────────────────────────────────────────────
uint16_t modbusCRC(const uint8_t *buf, uint8_t len) {
uint16_t crc = 0xFFFF;
for (uint8_t i = 0; i < len; i++) {
crc ^= (uint16_t)buf[i];
for (uint8_t j = 0; j < 8; j++)
crc = (crc & 1) ? (crc >> 1) ^ 0xA001 : crc >> 1;
}
return crc;
}
// ─────────────────────────────────────────────────────────────
// Build FC04 request
// ─────────────────────────────────────────────────────────────
void buildFC04Request(uint8_t *frame, uint8_t slaveId,
uint16_t regAddr, uint16_t regCount) {
frame[0] = slaveId;
frame[1] = 0x04;
frame[2] = regAddr >> 8;
frame[3] = regAddr & 0xFF;
frame[4] = regCount >> 8;
frame[5] = regCount & 0xFF;
uint16_t crc = modbusCRC(frame, 6);
frame[6] = crc & 0xFF;
frame[7] = crc >> 8;
}
// ─────────────────────────────────────────────────────────────
// Read sensor distance
// ─────────────────────────────────────────────────────────────
float readDistanceMM() {
while (rs485Serial.available()) rs485Serial.read();
uint8_t request[8];
buildFC04Request(request, MODBUS_SLAVE_ID,
SENSOR_REG_ADDR, SENSOR_REG_COUNT);
Serial.print("[TX] ");
for (int i = 0; i < 8; i++) {
if (request[i] < 0x10) Serial.print('0');
Serial.print(request[i], HEX);
Serial.print(' ');
}
Serial.println();
rs485Serial.write(request, sizeof(request));
rs485Serial.flush();
uint8_t response[EXPECTED_RESPONSE_LEN];
uint8_t bytesReceived = 0;
uint32_t deadline = millis() + MODBUS_RESPONSE_TIMEOUT_MS;
while (millis() < deadline && bytesReceived < EXPECTED_RESPONSE_LEN) {
if (rs485Serial.available())
response[bytesReceived++] = rs485Serial.read();
}
Serial.print("[RX] ");
for (int i = 0; i < bytesReceived; i++) {
if (response[i] < 0x10) Serial.print('0');
Serial.print(response[i], HEX);
Serial.print(' ');
}
Serial.println();
if (bytesReceived < EXPECTED_RESPONSE_LEN) {
Serial.println("[ERR] Timeout / incomplete response");
return NAN;
}
if (response[0] != MODBUS_SLAVE_ID) { Serial.println("[ERR] Wrong slave ID"); return NAN; }
if (response[1] == 0x84) { Serial.println("[ERR] Modbus exception"); return NAN; }
if (response[1] != 0x04) { Serial.println("[ERR] Wrong function code"); return NAN; }
uint16_t rxCRC = (uint16_t)response[7] | ((uint16_t)response[8] << 8);
uint16_t calcCRC = modbusCRC(response, 7);
if (rxCRC != calcCRC) { Serial.println("[ERR] CRC mismatch"); return NAN; }
uint16_t raw = ((uint16_t)response[5] << 8) | response[6];
return raw * 0.001f;
}
// ─────────────────────────────────────────────────────────────
// setup()
// ─────────────────────────────────────────────────────────────
void setup() {
Serial.begin(115200);
delay(2000);
Serial.println("\n=== BOJKE BL-30NZ-485 on Waveshare ESP32-S3-POE-ETH-8DI-8DO ===");
// I2C init
Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
Serial.println("I2C initialised");
// TCA9554 init — all outputs, all HIGH (matches Waveshare Dout_Init)
TCA9554_Init(0x00, 0xFF);
Serial.println("TCA9554 initialised");
// RS485 init — exactly as Waveshare demo
rs485Serial.begin(MODBUS_BAUD, SERIAL_8N1, RXD1, TXD1);
if (!rs485Serial.setPins(-1, -1, -1, TXD1EN))
Serial.println("[WARN] Failed to set TXDEN pin");
if (!rs485Serial.setMode(UART_MODE_RS485_HALF_DUPLEX))
Serial.println("[WARN] Failed to set RS485 half-duplex mode");
Serial.println("RS485 initialised");
Serial.println("================================================================\n");
}
// ─────────────────────────────────────────────────────────────
// loop()
// ─────────────────────────────────────────────────────────────
void loop() {
float distance = readDistanceMM();
if (!std::isnan(distance)) {
if (distance >= 25.0f && distance <= 35.0f) {
Serial.print("Distance: ");
Serial.print(distance, 3);
Serial.println(" mm");
} else {
Serial.println("Distance: OUT OF RANGE");
}
} else {
Serial.println("Distance: READ ERROR");
}
Serial.println();
delay(POLL_INTERVAL_MS);
}
No comments:
Post a Comment