オリエンタルモータ社のブラシレスモータ BLVシリーズ Rタイプを使っている
そのドライバ、BLDC-KRDという製品はCAN通信かMODBUS(RS-485)をつかってコマンドを送ることで
位置制御や速度制御が可能。
今回は（まだ途上だが） PCに繋いだESP32マイコン（ESP32-Devkit)にテキストでコマンドを送ると
それに応じてドライバにコマンドを送ってくれるプログラムを作成した。
使ったのは、amazonで購入できるESP32マイコンボード ： Freenove ESP32 ボード という製品と
amazonで売られている MAX485モジュールだ
ドライバには主電源48ボルトを与え、通信電源に24ボルトを与えた。
16番ピンにRO
17番ピンにDI
4番ピンに DE/REを接続
PCからESP32へ送れるコマンドは
サーボオン ：SON,ID
サーボオフ ：SOFF,ID
モータ停止 ：STOP,ID
一定速度制御：SPD,1,dir,rpm,acc_ms,dec_ms
dir: 0=逆転 / 1=正転
rpm: 0〜9999（r/min）
acc_ms / dec_ms: ms（例：1000=1.0s、2500=2.5s）
である。
プログラムソースは次
#include <Arduino.h>
#define RS485_RX 16
#define RS485_TX 17
#define RS485_DE 4
#define MODBUS_TIMEOUT_MS 3000
HardwareSerial RS485(2);
// ===== 状態管理 =====
enum MB_STATE { MB_IDLE, MB_WAIT };
static MB_STATE mb_state = MB_IDLE;
static uint8_t rxBuf[256];
static int rxLen = 0;
static unsigned long requestTime = 0;
enum PENDING_KIND { PK_NONE, PK_SON, PK_SOFF, PK_SPD, PK_STOP, PK_STAT_POSVEL, PK_STAT_ALARM };
static PENDING_KIND pendingKind = PK_NONE;
static uint8_t pendingId = 0;
static bool son_state[256] = {false};
// 直近のプロファイル（STOPで使う）
static uint32_t lastAccMs[256];
static uint32_t lastDecMs[256];
static uint32_t lastTorque[256];
// STAT用の一時保持
static int32_t stat_pos_step = 0;
static int16_t stat_vel_rpm = 0;
// ================= CRC16 (Modbus) =================
static uint16_t modbusCRC(const uint8_t *buf, int len) {
uint16_t crc = 0xFFFF;
for (int pos = 0; pos < len; pos++) {
crc ^= (uint16_t)buf[pos];
for (int i = 0; i < 8; i++) {
if (crc & 1) { crc >>= 1; crc ^= 0xA001; }
else { crc >>= 1; }
}
}
return crc;
}
// ================= RS485 send =================
static void rs485Send(const uint8_t *frame, int len) {
digitalWrite(RS485_DE, HIGH);
delayMicroseconds(50);
RS485.write(frame, len);
RS485.flush();
delayMicroseconds(50);
digitalWrite(RS485_DE, LOW);
}
// ================= トランザクション開始（非ブロッキング） =================
static bool startTransaction(const uint8_t *frameNoCrc, int lenNoCrc, PENDING_KIND kind, uint8_t id) {
if (mb_state != MB_IDLE) return false;
static uint8_t txBuf[256];
int txLen = 0;
memcpy(txBuf, frameNoCrc, lenNoCrc);
txLen = lenNoCrc;
uint16_t crc = modbusCRC(txBuf, txLen);
txBuf[txLen++] = crc & 0xFF; // CRC Lo
txBuf[txLen++] = (crc >> 8) & 0xFF; // CRC Hi
rs485Send(txBuf, txLen);
rxLen = 0;
requestTime = millis();
pendingKind = kind;
pendingId = id;
mb_state = MB_WAIT;
return true;
}
// 32bit値を HighWord & Big-Endian で詰める（MSB→LSB）
static void put_u32_be(uint8_t *p, uint32_t v) {
p[0] = (v >> 24) & 0xFF;
p[1] = (v >> 16) & 0xFF;
p[2] = (v >> 8) & 0xFF;
p[3] = (v >> 0) & 0xFF;
}
// ================= リモートI/O（基準）へ 2レジスタ書込み（0x007C〜） =================
static bool writeRemoteIO_Base(uint8_t id, uint16_t hiWord, uint16_t loWord, PENDING_KIND kind) {
uint8_t fr[11];
fr[0] = id;
fr[1] = 0x10;
fr[2] = 0x00;
fr[3] = 0x7C;
fr[4] = 0x00;
fr[5] = 0x02;
fr[6] = 0x04;
fr[7] = (hiWord >> 8) & 0xFF;
fr[8] = (hiWord >> 0) & 0xFF;
fr[9] = (loWord >> 8) & 0xFF;
fr[10] = (loWord >> 0) & 0xFF;
return startTransaction(fr, sizeof(fr), kind, id);
}
// ================= ダイレクトデータ運転：0x005A から 14レジスタ一括書込み =================
static bool writeDirectData14(uint8_t id,
uint32_t opMode,
int32_t position_step,
int32_t speed_rpm_signed,
uint32_t acc_ms,
uint32_t dec_ms,
uint32_t torque_0p1pct,
uint32_t trigger,
PENDING_KIND kind) {
uint8_t fr[7 + 28];
fr[0] = id;
fr[1] = 0x10;
fr[2] = 0x00;
fr[3] = 0x5A; // start 0x005A
fr[4] = 0x00;
fr[5] = 0x0E; // 14 registers
fr[6] = 0x1C; // 28 bytes
uint8_t *d = &fr[7];
put_u32_be(d + 0, opMode);
put_u32_be(d + 4, (uint32_t)position_step);
put_u32_be(d + 8, (uint32_t)speed_rpm_signed);
put_u32_be(d + 12, acc_ms);
put_u32_be(d + 16, dec_ms);
put_u32_be(d + 20, torque_0p1pct);
put_u32_be(d + 24, trigger);
return startTransaction(fr, sizeof(fr), kind, id);
}
// ================= Read Holding Registers (0x03) =================
static bool readHolding(uint8_t id, uint16_t startAddr, uint16_t count, PENDING_KIND kind) {
uint8_t fr[6];
fr[0] = id;
fr[1] = 0x03;
fr[2] = (startAddr >> 8) & 0xFF;
fr[3] = (startAddr >> 0) & 0xFF;
fr[4] = (count >> 8) & 0xFF;
fr[5] = (count >> 0) & 0xFF;
return startTransaction(fr, sizeof(fr), kind, id);
}
// ================= 応答処理（非ブロッキング） =================
static void handleModbus() {
if (mb_state != MB_WAIT) return;
while (RS485.available()) {
if (rxLen < (int)sizeof(rxBuf)) rxBuf[rxLen++] = (uint8_t)RS485.read();
else (void)RS485.read();
}
// 最低でもCRC含め5バイト以上になりがち
if (rxLen >= 5) {
// CRC検証は「ある程度受けてから」（0x10正常応答は8バイト、0x03は 5+N が多い）
if (rxLen >= 8) {
uint16_t rcrc = (uint16_t)rxBuf[rxLen - 2] | ((uint16_t)rxBuf[rxLen - 1] << 8);
uint16_t ccrc = modbusCRC(rxBuf, rxLen - 2);
if (rcrc == ccrc) {
uint8_t fc = rxBuf[1];
// 例外応答
if (fc & 0x80) {
Serial.printf("EXC,%02X,%02X\n", fc, rxBuf[2]);
pendingKind = PK_NONE;
mb_state = MB_IDLE;
return;
}
// ====== 0x10（書込み） ======
if (fc == 0x10) {
Serial.println("OK");
if (pendingKind == PK_SON) son_state[pendingId] = true;
if (pendingKind == PK_SOFF) son_state[pendingId] = false;
pendingKind = PK_NONE;
mb_state = MB_IDLE;
return;
}
// ====== 0x03（読出し） ======
if (fc == 0x03) {
// [id][03][byteCount][data...][crcLo][crcHi]
if (rxLen < 5) { /* wait */ }
uint8_t byteCount = rxBuf[2];
// 期待長： 3 + byteCount + 2
int expected = 3 + byteCount + 2;
if (rxLen < expected) return; // まだ受信途中
// POS+VEL（0x0066から3レジスタ）
if (pendingKind == PK_STAT_POSVEL) {
// data: reg0..reg2 = 6 bytes
if (byteCount >= 6) {
uint16_t r0 = (rxBuf[3] << 8) | rxBuf[4]; // 0x0066 hi
uint16_t r1 = (rxBuf[5] << 8) | rxBuf[6]; // 0x0067 lo
uint16_t r2 = (rxBuf[7] << 8) | rxBuf[8]; // 0x0068 vel
stat_pos_step = (int32_t)((uint32_t)r0 << 16 | (uint32_t)r1);
stat_vel_rpm = (int16_t)r2;
} else {
Serial.println("EXC,LOCAL,STAT_SHORT");
pendingKind = PK_NONE;
mb_state = MB_IDLE;
return;
}
// 続けてアラーム（0x0081 1レジスタ）を読む
mb_state = MB_IDLE; // 次を開始できるよう一旦IDLEへ
pendingKind = PK_NONE;
if (!readHolding(pendingId, 0x0081, 1, PK_STAT_ALARM)) {
Serial.println("EXC,LOCAL,BUSY");
}
return;
}
// ALARM（0x0081）
if (pendingKind == PK_STAT_ALARM) {
uint16_t alarm = 0;
if (byteCount >= 2) {
alarm = (rxBuf[3] << 8) | rxBuf[4];
}
Serial.printf("STAT,%u,%ld,%d,%02X\n",
(unsigned)pendingId,
(long)stat_pos_step,
(int)stat_vel_rpm,
(unsigned)(alarm & 0xFF));
pendingKind = PK_NONE;
mb_state = MB_IDLE;
return;
}
// それ以外の0x03は今はOKだけ返す（拡張用）
Serial.println("OK");
pendingKind = PK_NONE;
mb_state = MB_IDLE;
return;
}
}
}
}
if (millis() - requestTime > MODBUS_TIMEOUT_MS) {
Serial.println("TIMEOUT");
pendingKind = PK_NONE;
mb_state = MB_IDLE;
}
}
void setup() {
Serial.begin(115200);
RS485.begin(38400, SERIAL_8N1, RS485_RX, RS485_TX);
pinMode(RS485_DE, OUTPUT);
digitalWrite(RS485_DE, LOW);
for (int i = 0; i < 256; i++) {
lastAccMs[i] = 1000;
lastDecMs[i] = 1000;
lastTorque[i] = 1000; // 100.0%
}
Serial.println("Ready: SON,id / SOFF,id / SPD,id,dir,rpm,acc_ms,dec_ms / STOP,id / STAT,id");
}
// CSVの int 取り出し（最大maxCount個）
static bool parseCsvInts(const String &s, int *out, int maxCount, int &count) {
count = 0;
int start = 0;
while (start < (int)s.length() && count < maxCount) {
int comma = s.indexOf(',', start);
String token = (comma < 0) ? s.substring(start) : s.substring(start, comma);
token.trim();
out[count++] = token.toInt();
if (comma < 0) break;
start = comma + 1;
}
return (count > 0);
}
void loop() {
handleModbus();
if (!Serial.available()) return;
String line = Serial.readStringUntil('\n');
line.trim();
auto busyReply = []() { Serial.println("EXC,LOCAL,BUSY"); };
auto badIdReply = []() { Serial.println("EXC,LOCAL,BAD_ID"); };
auto badArgReply = []() { Serial.println("EXC,LOCAL,BAD_ARG"); };
if (line.startsWith("SON,")) {
int id = line.substring(4).toInt();
if (id <= 0 || id > 255) { badIdReply(); return; }
if (!writeRemoteIO_Base((uint8_t)id, 0x0000, 0x0001, PK_SON)) busyReply();
return;
}
if (line.startsWith("SOFF,")) {
int id = line.substring(5).toInt();
if (id <= 0 || id > 255) { badIdReply(); return; }
if (!writeRemoteIO_Base((uint8_t)id, 0x0000, 0x0000, PK_SOFF)) busyReply();
return;
}
// SPD,id,dir,rpm,acc_ms,dec_ms
if (line.startsWith("SPD,")) {
int idx1 = line.indexOf(',');
if (idx1 < 0) { badArgReply(); return; }
String rest = line.substring(idx1 + 1);
int parts[5]; int pcnt = 0;
if (!parseCsvInts(rest, parts, 5, pcnt) || pcnt < 3) { badArgReply(); return; }
int id = parts[0];
int dir = parts[1];
int rpm = parts[2];
int acc = (pcnt >= 4) ? parts[3] : 1000;
int dec = (pcnt >= 5) ? parts[4] : acc;
if (id <= 0 || id > 255) { badIdReply(); return; }
if (!(dir == 0 || dir == 1)) { badArgReply(); return; }
if (rpm < 0 || rpm > 9999) { badArgReply(); return; }
if (acc < 0) acc = 0;
if (dec < 0) dec = 0;
if (!son_state[id]) { Serial.println("EXC,LOCAL,SON_REQUIRED"); return; }
// 連続運転（速度制御）：運転方式=48（0x30）
uint32_t opMode = 48;
int32_t pos_step = 0;
int32_t spd = (dir == 1) ? (int32_t)rpm : -(int32_t)rpm;
lastAccMs[id] = (uint32_t)acc;
lastDecMs[id] = (uint32_t)dec;
if (!writeDirectData14((uint8_t)id,
opMode,
pos_step,
spd,
lastAccMs[id],
lastDecMs[id],
lastTorque[id],
1,
PK_SPD)) {
busyReply();
}
return;
}
// STOP,id（減速停止：運転方式=0、速度=0）
if (line.startsWith("STOP,")) {
int id = line.substring(5).toInt();
if (id <= 0 || id > 255) { badIdReply(); return; }
if (!son_state[id]) { Serial.println("EXC,LOCAL,SON_REQUIRED"); return; }
uint32_t opMode = 0;
int32_t pos_step = 0;
int32_t spd = 0;
if (!writeDirectData14((uint8_t)id,
opMode,
pos_step,
spd,
lastAccMs[id],
lastDecMs[id],
lastTorque[id],
1,
PK_STOP)) {
busyReply();
}
return;
}
// STAT,id（位置＋速度＋アラーム）
if (line.startsWith("STAT,")) {
int id = line.substring(5).toInt();
if (id <= 0 || id > 255) { badIdReply(); return; }
// 0x0066から3レジスタ：位置(0x0066/0x0067) + 速度(0x0068)
if (!readHolding((uint8_t)id, 0x0066, 3, PK_STAT_POSVEL)) {
busyReply();
}
return;
}
Serial.println("EXC,LOCAL,NYI");
}