From da70d5274653480a7073340229aa7427714b01bd Mon Sep 17 00:00:00 2001 From: Suzu <47468734+Suzu-Gears@users.noreply.github.com> Date: Thu, 27 Nov 2025 16:42:43 +0900 Subject: [PATCH 1/3] fix examples --- .../1_SingleMotor_MIT_Control.ino | 157 ++++++++++++++++++ .../1_SingleMotor_PositionControl.ino | 136 --------------- .../2_DoubleMotor_MIT_Control.ino | 154 +++++++++++++++++ .../2_DoubleMotor_PositionControl.ino | 137 --------------- .../3_Advanced_ModeSwitching.ino | 129 ++++++++------ .../4_Utility_ReadWriteParams.ino | 77 ++++++--- 6 files changed, 443 insertions(+), 347 deletions(-) create mode 100644 examples/1_SingleMotor_MIT_Control/1_SingleMotor_MIT_Control.ino delete mode 100644 examples/1_SingleMotor_PositionControl/1_SingleMotor_PositionControl.ino create mode 100644 examples/2_DoubleMotor_MIT_Control/2_DoubleMotor_MIT_Control.ino delete mode 100644 examples/2_DoubleMotor_PositionControl/2_DoubleMotor_PositionControl.ino diff --git a/examples/1_SingleMotor_MIT_Control/1_SingleMotor_MIT_Control.ino b/examples/1_SingleMotor_MIT_Control/1_SingleMotor_MIT_Control.ino new file mode 100644 index 0000000..ac8b20c --- /dev/null +++ b/examples/1_SingleMotor_MIT_Control/1_SingleMotor_MIT_Control.ino @@ -0,0 +1,157 @@ +/** + * @file 1_SingleMotor_MIT_Control.ino + * @brief DAMIAOモーターを1つ、MITモードで動作させる基本的なサンプルコードです。 + * @details + * MITモードは、目標位置、目標速度、Pゲイン(Kp)、Dゲイン(Kd)、フィードフォワードトルクを + * 組み合わせてモーターを制御するモードです。 + * 出力トルク = Kp * (目標位置 - 現在位置) + Kd * (目標速度 - 現在速度) + フィードフォワードトルク + * + * このサンプルでは、以下の手順でモーターを制御します。 + * 1. CAN通信の初期化 + * 2. モーターの各種パラメータを取得 + * 3. モーターの制御モードを「MITモード」に設定 + * 4. 現在位置をゼロ点として設定 + * 5. モーターを有効化（起動） + * 6. MITモードの指令を送信（目標位置0速度0でバネダンパの挙動） + * 7. 100msごとにモーターの状態（位置、速度、トルクなど）をシリアルモニタに表示 + */ + +// ▼▼▼ Select Board ▼▼▼ 使うボードだけコメントアウトを外す +#define USE_BOARD_ARDUINO_R4 +// #define USE_BOARD_PICO +// #define USE_BOARD_ESP32 + +#if defined(USE_BOARD_ARDUINO_R4) +#include +// R4はCAN_TX/CAN_RXピンが固定のため、ピン設定は不要 + +#elif defined(USE_BOARD_ESP32) +#include // https://github.com/eyr1n/ESP32_TWAI +// 使用するマイコンに合わせてピン番号を変更してください +const gpio_num_t CAN_TX_PIN = 22; +const gpio_num_t CAN_RX_PIN = 21; + +#elif defined(USE_BOARD_PICO) +#include //https://github.com/eyr1n/RP2040PIO_CAN +// 使用するマイコンに合わせてピン番号を変更してください +const uint32_t CAN_TX_PIN = 0; // 連続してなくてもいい +const uint32_t CAN_RX_PIN = 1; // GP1とGP3みたいな組み合わせでも動く + +#else +#error "ボードが選択されていません。ファイルの先頭で USE_BOARD_... のどれか1つを有効にしてください。" +#endif + +//CANライブラリよりも下で呼び出す api/HardwareCAN.hが無いって言われる +#include // DAMIAOモーター制御ライブラリ +#include // ユーティリティ関数置き場 + +using namespace damiao; + +// ============================================= +// ユーザー設定項目 (User Settings) +// ============================================= +const uint32_t MASTER_ID = 0x00; // モーターのMasterID +const uint32_t SLAVE_ID = 0x09; // モーターのSlaveID +// ============================================= + +// モーターオブジェクトの作成 +Motor motor1(MASTER_ID, SLAVE_ID); + +// モーターのフィードバック情報をシリアルモニタに表示する関数 +void printFeedback(Motor& motor) { + Status status = motor.getStatus(); + Mode mode = motor.getMode(); + float pos = motor.getPosition(); + float vel = motor.getVelocity(); + float tau = motor.getTorque(); + + Serial.print("[FB] ID:"); + Serial.print(motor.getSlaveId()); + Serial.print(" Status:"); + Serial.print(statusToString(status)); // ステータスを文字列に変換して表示 + Serial.print(" Mode:"); + Serial.print(modeToString(mode)); // モードを文字列に変換して表示 + Serial.print(" Pos:"); + Serial.print(pos, 2); + Serial.print(" Vel:"); + Serial.print(vel, 2); + Serial.print(" Tau:"); + Serial.println(tau, 2); +} + +void setup() { + Serial.begin(115200); + // シリアルモニタが起動するまで最大5秒待機 + while (!Serial && millis() < 5000); + Serial.println("\n--- Single Motor MIT Control Example ---"); + + // 1. CAN通信の初期化 + bool can_ok = false; +#if defined(USE_BOARD_ARDUINO_R4) + can_ok = CAN.begin(CanBitRate::BR_1000k); + +#elif defined(USE_BOARD_ESP32) + can_ok = CAN.begin(CanBitRate::BR_1000k, CAN_TX_PIN, CAN_RX_PIN); + +#elif defined(USE_BOARD_PICO) + CAN.setTX(CAN_TX_PIN); + CAN.setRX(CAN_RX_PIN); + can_ok = CAN.begin(CanBitRate::BR_1000k); +#endif + + if (can_ok) { + Serial.println("CAN bus initialized successfully."); + } else { + Serial.println("CAN bus initialization failed!"); + while (1); // CAN通信が開始できない場合はここで停止 + } + motor1.setCAN(&CAN); // モーターにCANインスタンスを渡す + motor1.disable(); // 念のためモーターを無効化 + + // 2. モーターの各種パラメータを取得 + motor1.initialize(); + + // 3. モーターの制御モードを設定 + Serial.print("Setting control mode to MIT..."); + if (motor1.setControlMode(Mode::MIT)) { + Serial.println("Success."); + } else { + Serial.println("Failed."); + } + + // 4. 現在位置をゼロ点として設定 + Serial.print("Setting current position as zero..."); + if (motor1.setZeroPosition()) { + Serial.println("Success."); + } else { + Serial.println("Failed."); + } + + // 5. モーターを有効化（起動） + Serial.print("Enabling motor..."); + if (motor1.enable()) { + Serial.println("Success."); + } else { + Serial.println("Failed."); + } + + Serial.println("\nSetup complete. Motor is running."); +} + +void loop() { + // CANバス上のメッセージを処理し、モーターの内部状態（位置、速度など）を更新 + motor1.update(); + + // 6. MITモードで指令を送信（目標位置0速度0でバネダンパの挙動） + motor1.sendMIT(0.0f, 0.0f, 1.0f, 1.0f, 0.0f); + + // 7. 定期的にモーターの状態をシリアルモニタに表示 + const unsigned long FEEDBACK_INTERVAL_MS = 100UL; // 100msごとに表示 + static unsigned long lastFeedbackMillis = 0; + unsigned long now = millis(); + if (now - lastFeedbackMillis >= FEEDBACK_INTERVAL_MS) { + lastFeedbackMillis = now; + printFeedback(motor1); + } + delay(1); +} diff --git a/examples/1_SingleMotor_PositionControl/1_SingleMotor_PositionControl.ino b/examples/1_SingleMotor_PositionControl/1_SingleMotor_PositionControl.ino deleted file mode 100644 index 788e65c..0000000 --- a/examples/1_SingleMotor_PositionControl/1_SingleMotor_PositionControl.ino +++ /dev/null @@ -1,136 +0,0 @@ -/** - * @file 1_SingleMotor_PositionControl.ino - * @brief DAMIAOモーターを1つ、位置制御モードで動作させる基本的なサンプルコードです。 - * @details - * このサンプルでは、以下の手順でモーターを制御します。 - * 1. CAN通信の初期化 - * 2. モーターの位置・速度・トルクの計算用パラメータを取得 - * 3. モーターの制御モードを「位置・速度制御モード」に設定 - * 4. 現在位置をゼロ点として設定 - * 5. モーターを有効化（起動） - * 6. 指定した目標位置（ラジアン単位）までモーターを回転させる - * 7. 500msごとにモーターの状態（位置、速度、トルクなど）をシリアルモニタに表示 - */ -#include // RP2040用のCAN通信ライブラリ - -#include // DAMIAOモーター制御ライブラリ -#include // 便利なユーティリティ関数 - -using namespace damiao; - -// ============================================= -// ユーザー設定項目 (User Settings) -// ============================================= - -// --- CAN通信設定 --- -// 使用するマイコンに合わせてピン番号を変更してください -const uint8_t CAN_TX_PIN = 0; // CANトランシーバーのTXピン -const uint8_t CAN_RX_PIN = 1; // CANトランシーバーのRXピン - -// --- モーター設定 --- -const uint32_t MASTER_ID = 0x00; // モーターのMasterID -const uint32_t SLAVE_ID = 0x09; // モーターのSlaveID - -// --- 制御目標値 --- -const float TARGET_POSITION_RAD = M_PI; // 目標位置 (ラジアン単位, M_PI = 180°) -const float VELOCITY_LIMIT_RPS = 1.0f; // 速度制限 (rad/s) - -// ============================================= - -// モーターオブジェクトの作成 -Motor motor1(MASTER_ID, SLAVE_ID); - -// 最後にフィードバックを表示した時刻 -static unsigned long lastFeedbackMillis = 0; -const unsigned long FEEDBACK_INTERVAL_MS = 500UL; // 500msごとに表示 - -// モーターのフィードバック情報をシリアルモニタに表示する関数 -void printFeedback() { - // motor1.update() を loop内で呼び出すことで、これらの値は最新に保たれます - Status status = motor1.getStatus(); - Mode mode = motor1.getMode(); - float pos = motor1.getPosition(); - float vel = motor1.getVelocity(); - float tau = motor1.getTorque(); - - Serial.print("[FB] ID:"); - Serial.print(motor1.getSlaveId()); - Serial.print(" Status:"); - Serial.print(statusToString(status)); // ステータスを文字列に変換して表示 - Serial.print(" Mode:"); - Serial.print(modeToString(mode)); // モードを文字列に変換して表示 - Serial.print(" Pos:"); - Serial.print(pos, 2); // 小数点以下2桁まで表示 - Serial.print(" Vel:"); - Serial.print(vel, 2); - Serial.print(" Tau:"); - Serial.println(tau, 2); -} - -void setup() { - Serial.begin(115200); - // シリアルモニタが起動するまで最大5秒待機 - while (!Serial && millis() < 5000); - Serial.println("\n--- Single Motor Position Control Example ---"); - - // 1. CAN通信の初期化 - CAN.setTX(CAN_TX_PIN); - CAN.setRX(CAN_RX_PIN); - if (CAN.begin(CanBitRate::BR_1000k)) { - Serial.println("CAN bus initialized successfully."); - } else { - Serial.println("CAN bus initialization failed!"); - while (1); // CAN通信が開始できない場合はここで停止 - } - motor1.setCAN(&CAN); // モーターにCANインスタンスを渡す - delay(100); - - // 2. モーターの位置・速度・トルクの計算用パラメータを取得 - motor1.initialize(); - - // 3. モーターの制御モードを設定 - Serial.print("Setting control mode to POS_VEL..."); - if (motor1.setControlMode(Mode::POS_VEL)) { - Serial.println("Success."); - } else { - Serial.println("Failed."); - } - delay(100); - - // 4. 現在位置をゼロ点として設定 - Serial.print("Setting current position as zero..."); - if (motor1.setZeroPosition()) { - Serial.println("Success."); - } else { - Serial.println("Failed."); - } - delay(100); - - // 5. モーターを有効化（起動） - Serial.print("Enabling motor..."); - if (motor1.enable()) { - Serial.println("Success."); - } else { - Serial.println("Failed."); - } - - Serial.println("\nSetup complete. Motor is running."); -} - -void loop() { - // CANバス上のメッセージを処理し、モーターの内部状態（位置、速度など）を更新します。 - // この関数はできるだけ頻繁に呼び出してください。 - motor1.update(); - - // 6. 指定した目標位置にモーターを回転させる - motor1.sendPosition(TARGET_POSITION_RAD, VELOCITY_LIMIT_RPS); - - // 7. 定期的にモーターの状態をシリアルモニタに表示 - unsigned long now = millis(); - if (now - lastFeedbackMillis >= FEEDBACK_INTERVAL_MS) { - lastFeedbackMillis = now; - printFeedback(); - } - - delay(1); // CPU負荷を軽減するための短い待機 -} diff --git a/examples/2_DoubleMotor_MIT_Control/2_DoubleMotor_MIT_Control.ino b/examples/2_DoubleMotor_MIT_Control/2_DoubleMotor_MIT_Control.ino new file mode 100644 index 0000000..e4874bf --- /dev/null +++ b/examples/2_DoubleMotor_MIT_Control/2_DoubleMotor_MIT_Control.ino @@ -0,0 +1,154 @@ +/** + * @file 2_DoubleMotor_MIT_Control.ino + * @brief DAMIAOモーターを2つ、MITモードで動作させるサンプルコードです。 + * @details + * このサンプルでは、MotorManagerクラスを使って複数のモーターを管理する方法を示します。 + * 2つのモーターが、MIT制御されます。 + * 1. CAN通信の初期化 + * 2. 2つのモーターを管理するMotorManagerを作成 + * 3. モーターの各種パラメータを取得 + * 4. 両方のモーターを「MITモード」に設定 + * 5. 両方のモーターの現在位置をゼロ点として設定 + * 6. 両方のモーターを有効化（起動） + * 7. MITモードの指令を送信（目標位置0速度0でバネダンパの挙動） + * 8. 100msごとにモーターの状態（位置、速度、トルクなど）をシリアルモニタに表示 + */ + +// ▼▼▼ Select Board ▼▼▼ 使うボードだけコメントアウトを外す +#define USE_BOARD_ARDUINO_R4 +// #define USE_BOARD_PICO +// #define USE_BOARD_ESP32 + +#if defined(USE_BOARD_ARDUINO_R4) +#include +// R4はCAN_TX/CAN_RXピンが固定のため、ピン設定は不要 + +#elif defined(USE_BOARD_ESP32) +#include // https://github.com/eyr1n/ESP32_TWAI +// 使用するマイコンに合わせてピン番号を変更してください +const gpio_num_t CAN_TX_PIN = 22; +const gpio_num_t CAN_RX_PIN = 21; + +#elif defined(USE_BOARD_PICO) +#include //https://github.com/eyr1n/RP2040PIO_CAN +// 使用するマイコンに合わせてピン番号を変更してください +const uint32_t CAN_TX_PIN = 0; // 連続してなくてもいい +const uint32_t CAN_RX_PIN = 1; // GP1とGP3みたいな組み合わせでも動く + +#else +#error "ボードが選択されていません。ファイルの先頭で USE_BOARD_... のどれか1つを有効にしてください。" +#endif + +//CANライブラリよりも下で呼び出す api/HardwareCAN.hが無いって言われる +#include // DAMIAOモーター制御ライブラリ +#include // ユーティリティ関数置き場 + +using namespace damiao; + +// ============================================= +// ユーザー設定項目 (User Settings) +// ============================================= +const uint32_t MASTER_ID = 0x00; // モーターのMasterID +const uint32_t MOTOR_1_SLAVE_ID = 0x09; // 1台目のモーターのSlaveID +const uint32_t MOTOR_2_SLAVE_ID = 0x0a; // 2台目のモーターのSlaveID +// ============================================= + +// モーターオブジェクトの作成 +Motor motor1(MASTER_ID, MOTOR_1_SLAVE_ID); +Motor motor2(MASTER_ID, MOTOR_2_SLAVE_ID); + +// 複数のモーターを管理するMotorManagerを作成 +// <>内の数字は管理するモーターの数 +MotorManager<2> motorManager(&CAN, &motor1, &motor2); // カンマ区切りで複数のモーターを渡す + +void printFeedback(Motor& motor) { + Status status = motor.getStatus(); + Mode mode = motor.getMode(); + float pos = motor.getPosition(); + float vel = motor.getVelocity(); + float tau = motor.getTorque(); + + Serial.print("[FB] ID:"); + Serial.print(motor.getSlaveId()); + Serial.print(" Status:"); + Serial.print(statusToString(status)); + Serial.print(" Mode:"); + Serial.print(modeToString(mode)); + Serial.print(" Pos:"); + Serial.print(pos, 2); + Serial.print(" Vel:"); + Serial.print(vel, 2); + Serial.print(" Tau:"); + Serial.println(tau, 2); +} + +void setup() { + Serial.begin(115200); + // シリアルモニタが起動するまで最大5秒待機 + while (!Serial && millis() < 5000); + Serial.println("\n--- Single Motor MIT Control Example ---"); + + // 1. CAN通信の初期化 + bool can_ok = false; +#if defined(USE_BOARD_ARDUINO_R4) + can_ok = CAN.begin(CanBitRate::BR_1000k); + +#elif defined(USE_BOARD_ESP32) + can_ok = CAN.begin(CanBitRate::BR_1000k, CAN_TX_PIN, CAN_RX_PIN); + +#elif defined(USE_BOARD_PICO) + CAN.setTX(CAN_TX_PIN); + CAN.setRX(CAN_RX_PIN); + can_ok = CAN.begin(CanBitRate::BR_1000k); +#endif + + if (can_ok) { + Serial.println("CAN bus initialized successfully."); + } else { + Serial.println("CAN bus initialization failed!"); + while (1); // CAN通信が開始できない場合はここで停止 + } + + // 2. MotorManagerを使ってモーターを一括でセットアップ + Serial.println("Setting up motors via MotorManager..."); + motorManager.disableAll(); // 念のため全モーターを無効化 + + // 3. 全モーターの各種パラメータを取得 + motorManager.initializeAll(); + + // 4. 全モーターの制御モードを設定 + // forループでも可能 + for (size_t i = 0; i < motorManager.size(); ++i) { + motorManager.getMotor(i).setControlMode(Mode::MIT); + } + + // 5. 全モーターの現在位置をゼロ点に設定 + // Motorオブジェクトを直接使っても可能 + motor1.setZeroPosition(); + motor2.setZeroPosition(); + + // 6. 全モーターを有効化 + motorManager.enableAll(); + + Serial.println("\nSetup complete. Motors are running."); +} + +void loop() { + // CANバス上のメッセージを処理し、モーターの内部状態（位置、速度など）を一括更新 + motorManager.update(); + + // 7. MITモードで指令を送信（目標位置0速度0でバネダンパの挙動） + motor1.sendMIT(0.0f, 0.0f, 1.0f, 1.0f, 0.0f); + motor2.sendMIT(0.0f, 0.0f, 1.0f, 1.0f, 0.0f); + + // 8. 定期的にモーターの状態をシリアルモニタに表示 + const unsigned long FEEDBACK_INTERVAL_MS = 100UL; // 100msごとに表示 + static unsigned long lastFeedbackMillis = 0; + unsigned long now = millis(); + if (now - lastFeedbackMillis >= FEEDBACK_INTERVAL_MS) { + lastFeedbackMillis = now; + printFeedback(motor1); + printFeedback(motor2); + } + delay(1); +} diff --git a/examples/2_DoubleMotor_PositionControl/2_DoubleMotor_PositionControl.ino b/examples/2_DoubleMotor_PositionControl/2_DoubleMotor_PositionControl.ino deleted file mode 100644 index 1f9f222..0000000 --- a/examples/2_DoubleMotor_PositionControl/2_DoubleMotor_PositionControl.ino +++ /dev/null @@ -1,137 +0,0 @@ -/** - * @file 2_DoubleMotor_PositionControl.ino - * @brief DAMIAOモーターを2つ、位置制御モードで動作させるサンプルコードです。 - * @details - * このサンプルでは、MotorManagerクラスを使って複数のモーターを効率的に管理する方法を示します。 - * 2つのモーターが、それぞれ反対方向にサイン波を描くように動きます。 - * 1. CAN通信の初期化 - * 2. 2つのモーターを管理するMotorManagerを作成 - * 3. 両方のモーターを「位置・速度制御モード」に設定 - * 4. 両方のモーターの現在位置をゼロ点として設定 - * 5. 両方のモーターを有効化（起動） - * 6. loop内で時間経過に合わせてサイン波の目標位置を計算し、各モーターに指令を送信 - * 7. 500msごとに両方のモーターの状態をシリアルモニタに表示 - */ -#include // RP2040用のCAN通信ライブラリ - -#include // DAMIAOモーター制御ライブラリ -#include // 便利なユーティリティ関数 - -using namespace damiao; - -// ============================================= -// ユーザー設定項目 (User Settings) -// ============================================= - -// --- CAN通信設定 --- -const uint8_t CAN_TX_PIN = 0; -const uint8_t CAN_RX_PIN = 1; - -// --- モーター設定 --- -const uint32_t MASTER_ID = 0x00; -const uint32_t MOTOR_1_SLAVE_ID = 0x09; // 1台目のモーターのSlaveID -const uint32_t MOTOR_2_SLAVE_ID = 0x0a; // 2台目のモーターのSlaveID - -// --- 制御目標値 --- -const float AMPLITUDE_RAD = M_PI / 2.0f; // 動作の振幅 (ラジアン単位, PI/2 = 90°) -const float FREQUENCY_HZ = 0.2f; // 動作の周波数 (Hz) -const float VELOCITY_LIMIT_RPS = 2.0f; // 速度制限 (rad/s) - -// ============================================= - -// モーターオブジェクトの作成 -Motor motor1(MASTER_ID, MOTOR_1_SLAVE_ID); -Motor motor2(MASTER_ID, MOTOR_2_SLAVE_ID); - -// 2つのモーターを管理するMotorManagerを作成 -// <>内の数字は管理するモーターの数 -MotorManager<2> motorManager(&CAN, &motor1, &motor2); // カンマ区切りで複数のモーターを渡す - -// 最後にフィードバックを表示した時刻 -static unsigned long lastFeedbackMillis = 0; -const unsigned long FEEDBACK_INTERVAL_MS = 500UL; - -// モーターのフィードバック情報をシリアルモニタに表示する関数 -void printFeedback(Motor& motor) { - Status status = motor.getStatus(); - Mode mode = motor.getMode(); - float pos = motor.getPosition(); - float vel = motor.getVelocity(); - float tau = motor.getTorque(); - - Serial.print("[FB] ID:"); - Serial.print(motor.getSlaveId()); - Serial.print(" Pos:"); - Serial.print(pos, 2); - Serial.print(" Vel:"); - Serial.print(vel, 2); - Serial.print(" Tau:"); - Serial.println(tau, 2); -} - -void setup() { - Serial.begin(115200); - while (!Serial && millis() < 5000); - Serial.println("\n--- Double Motor Position Control Example ---"); - - // 1. CAN通信の初期化 - CAN.setTX(CAN_TX_PIN); - CAN.setRX(CAN_RX_PIN); - if (CAN.begin(CanBitRate::BR_1000k)) { - Serial.println("CAN bus initialized successfully."); - } else { - Serial.println("CAN bus initialization failed!"); - while (1); // CAN通信が開始できない場合はここで停止 - } - delay(100); - - // 2. MotorManagerを使ってモーターを一括でセットアップ - Serial.println("Setting up motors via MotorManager..."); - - // 全モーターのパラメータを初期化 - motorManager.initializeAll(); - - // 全モーターを位置・速度制御モードに設定 - // forループでも可能 - for (size_t i = 0; i < motorManager.size(); ++i) { - motorManager.getMotor(i).setControlMode(Mode::POS_VEL); - } - - // 全モーターの現在位置をゼロ点に設定 - // Motorオブジェクトを直接使っても可能 - motor1.setZeroPosition(); - motor2.setZeroPosition(); - - // 全モーターを有効化 - motorManager.enableAll(); - - Serial.println("\nSetup complete. Motors are running."); -} - -void loop() { - // 6. CANメッセージを処理し、全モーターの状態を更新 - motorManager.update(); - - // 7. 時間に応じてサイン波の目標位置を計算 - unsigned long currentTime = millis(); - float angle = 2.0f * M_PI * FREQUENCY_HZ * (currentTime / 1000.0f); - float targetPos1 = AMPLITUDE_RAD * sin(angle); - float targetPos2 = -AMPLITUDE_RAD * sin(angle); // 逆方向に動かす - - // 各モーターに目標位置を送信 - motor1.sendPosition(targetPos1, VELOCITY_LIMIT_RPS); - motor2.sendPosition(targetPos2, VELOCITY_LIMIT_RPS); - - // 8. 定期的にモーターの状態を表示 - unsigned long now = millis(); - if (now - lastFeedbackMillis >= FEEDBACK_INTERVAL_MS) { - lastFeedbackMillis = now; - Serial.println("--- Feedback ---"); - // MotorManagerを使って、管理下の全モーターの情報を表示 - for (size_t i = 0; i < motorManager.size(); ++i) { - printFeedback(motorManager.getMotor(i)); - } - } - - delay(1); -} diff --git a/examples/3_Advanced_ModeSwitching/3_Advanced_ModeSwitching.ino b/examples/3_Advanced_ModeSwitching/3_Advanced_ModeSwitching.ino index ce668cb..44dc7a0 100644 --- a/examples/3_Advanced_ModeSwitching/3_Advanced_ModeSwitching.ino +++ b/examples/3_Advanced_ModeSwitching/3_Advanced_ModeSwitching.ino @@ -21,49 +21,64 @@ * モードを切り替える際には、一度モーターを disable し、再度 enable するのが安全です。 * このサンプルでは、モード切替時に自動でゼロ点をリセットしています。 */ -#include -#include -#include +// ▼▼▼ Select Board ▼▼▼ 使うボードだけコメントアウトを外す +#define USE_BOARD_ARDUINO_R4 +// #define USE_BOARD_PICO +// #define USE_BOARD_ESP32 + +#if defined(USE_BOARD_ARDUINO_R4) +#include +// R4はCAN_TX/CAN_RXピンが固定のため、ピン設定は不要 + +#elif defined(USE_BOARD_ESP32) +#include // https://github.com/eyr1n/ESP32_TWAI +// 使用するマイコンに合わせてピン番号を変更してください +const gpio_num_t CAN_TX_PIN = 22; +const gpio_num_t CAN_RX_PIN = 21; + +#elif defined(USE_BOARD_PICO) +#include //https://github.com/eyr1n/RP2040PIO_CAN +// 使用するマイコンに合わせてピン番号を変更してください +const uint32_t CAN_TX_PIN = 0; // 連続してなくてもいい +const uint32_t CAN_RX_PIN = 1; // GP1とGP3みたいな組み合わせでも動く + +#else +#error "ボードが選択されていません。ファイルの先頭で USE_BOARD_... のどれか1つを有効にしてください。" +#endif + +//CANライブラリよりも下で呼び出す api/HardwareCAN.hが無いって言われる +#include // DAMIAOモーター制御ライブラリ +#include // ユーティリティ関数置き場 using namespace damiao; // ============================================= // ユーザー設定項目 (User Settings) // ============================================= -const uint8_t CAN_TX_PIN = 0; -const uint8_t CAN_RX_PIN = 1; - -const uint32_t MASTER_ID = 0x00; -const uint32_t MOTOR_SLAVE_ID = 0x09; +const uint32_t MASTER_ID = 0x00; // モーターのMasterID +const uint32_t SLAVE_ID = 0x09; // モーターのSlaveID // ============================================= -// モーターオブジェクト -Motor motor1(MASTER_ID, MOTOR_SLAVE_ID); +// モーターオブジェクトの作成 +Motor motor1(MASTER_ID, SLAVE_ID); // 現在の制御モードを管理するための変数 static Mode currentState = Mode::POS_VEL; - // 最後にモードを切り替えた時刻 static unsigned long lastSwitchMillis = 0; // モードを切り替える間隔 (ミリ秒) const unsigned long SWITCH_INTERVAL_MS = 10000UL; -// 最後にフィードバックを表示した時刻 -static unsigned long lastFeedbackMillis = 0; -const unsigned long FEEDBACK_INTERVAL_MS = 500UL; - - -// モーターのフィードバック情報を表示する関数 -void printFeedback() { - Status status = motor1.getStatus(); - Mode mode = motor1.getMode(); - float pos = motor1.getPosition(); - float vel = motor1.getVelocity(); - float tau = motor1.getTorque(); +void printFeedback(Motor& motor) { + Status status = motor.getStatus(); + Mode mode = motor.getMode(); + float pos = motor.getPosition(); + float vel = motor.getVelocity(); + float tau = motor.getTorque(); Serial.print("[FB] ID:"); - Serial.print(motor1.getSlaveId()); + Serial.print(motor.getSlaveId()); Serial.print(" Status:"); Serial.print(statusToString(status)); Serial.print(" Mode:"); @@ -77,14 +92,13 @@ void printFeedback() { } // 次の制御モードに切り替える関数 -void switchToNextState() { +void switchToNextState(Motor& motor) { Serial.println("\n--------------------"); Serial.print("Switching mode from "); - Serial.print(modeToString(motor1.getMode())); + Serial.print(modeToString(motor.getMode())); Serial.print(" to "); - // 安全のため、モード切替前に一度モーターを無効化 - motor1.disable(); + motor.disable(); // 安全のため、モード切替前に一度モーターを無効化 delay(100); // モードを順番に切り替える @@ -107,43 +121,51 @@ void switchToNextState() { } Serial.println(modeToString(currentState)); Serial.println("--------------------"); - - // 新しいモードを設定 - motor1.setControlMode(currentState); - - // ゼロ点をリセット - motor1.setZeroPosition(); - - // モーターを再度有効化 - motor1.enable(); + motor.setControlMode(currentState); // 新しいモードを設定 + motor.setZeroPosition(); // ゼロ点をリセット + motor.enable(); // モーターを再度有効化 } void setup() { Serial.begin(115200); + // シリアルモニタが起動するまで最大5秒待機 while (!Serial && millis() < 5000); - Serial.println("\n--- Advanced Mode Switching Example ---"); + Serial.println("\n--- Single Motor MIT Control Example ---"); + // 1. CAN通信の初期化 + bool can_ok = false; +#if defined(USE_BOARD_ARDUINO_R4) + can_ok = CAN.begin(CanBitRate::BR_1000k); + +#elif defined(USE_BOARD_ESP32) + can_ok = CAN.begin(CanBitRate::BR_1000k, CAN_TX_PIN, CAN_RX_PIN); + +#elif defined(USE_BOARD_PICO) CAN.setTX(CAN_TX_PIN); CAN.setRX(CAN_RX_PIN); - if (!CAN.begin(CanBitRate::BR_1000k)) { + can_ok = CAN.begin(CanBitRate::BR_1000k); +#endif + + if (can_ok) { + Serial.println("CAN bus initialized successfully."); + } else { Serial.println("CAN bus initialization failed!"); - while (1); + while (1); // CAN通信が開始できない場合はここで停止 } - motor1.setCAN(&CAN); - delay(100); + motor1.setCAN(&CAN); // モーターにCANインスタンスを渡す + motor1.disable(); // 念のためモーターを無効化 + + // 2. モーターの各種パラメータを取得 + motor1.initialize(); // 最初のモードを設定して有効化 Serial.print("Starting with mode: "); Serial.println(modeToString(currentState)); motor1.setControlMode(currentState); - delay(100); motor1.setZeroPosition(); - delay(100); motor1.enable(); - - // 最初の切り替え時刻を記録 - lastSwitchMillis = millis(); + lastSwitchMillis = millis(); // 最初の切り替え時刻を記録 } void loop() { @@ -153,7 +175,7 @@ void loop() { unsigned long now = millis(); if (now - lastSwitchMillis >= SWITCH_INTERVAL_MS) { lastSwitchMillis = now; - switchToNextState(); + switchToNextState(motor1); } // 現在のモードに応じた制御コマンドを送信 @@ -169,9 +191,9 @@ void loop() { break; case Mode::MIT: - // 【インピーダンス制御】目標位置 0, 速度 0, KP 5.0, KD 2.0, トルク 0 + // 【インピーダンス制御】目標位置 0, 速度 0, KP 1.0, KD 1.0, トルク 0 // この設定では、モーターはバネダンパの挙動を示します。 - motor1.sendMIT(0.0f, 0.0f, 5.0f, 2.0f, 0.0f); + motor1.sendMIT(0.0f, 0.0f, 1.0f, 1.0f, 0.0f); break; default: @@ -180,10 +202,11 @@ void loop() { } // 定期的にフィードバックを表示 + static unsigned long lastFeedbackMillis = 0; + const unsigned long FEEDBACK_INTERVAL_MS = 100UL; if (now - lastFeedbackMillis >= FEEDBACK_INTERVAL_MS) { lastFeedbackMillis = now; - printFeedback(); + printFeedback(motor1); } - - delay(10); // 少し長めのdelayでシリアル出力を見やすくする + delay(1); } diff --git a/examples/4_Utility_ReadWriteParams/4_Utility_ReadWriteParams.ino b/examples/4_Utility_ReadWriteParams/4_Utility_ReadWriteParams.ino index c4043df..076d376 100644 --- a/examples/4_Utility_ReadWriteParams/4_Utility_ReadWriteParams.ino +++ b/examples/4_Utility_ReadWriteParams/4_Utility_ReadWriteParams.ino @@ -16,51 +16,89 @@ * 電源を落としても設定を保持したい場合は、最後に `motor.saveParams()` を呼び出す必要があります。 * ただし、フラッシュメモリの寿命を縮める可能性があるため、頻繁に呼び出すことは避けてください。 */ -#include -#include -#include +// ▼▼▼ Select Board ▼▼▼ 使うボードだけコメントアウトを外す +#define USE_BOARD_ARDUINO_R4 +// #define USE_BOARD_PICO +// #define USE_BOARD_ESP32 + +#if defined(USE_BOARD_ARDUINO_R4) +#include +// R4はCAN_TX/CAN_RXピンが固定のため、ピン設定は不要 + +#elif defined(USE_BOARD_ESP32) +#include // https://github.com/eyr1n/ESP32_TWAI +// 使用するマイコンに合わせてピン番号を変更してください +const gpio_num_t CAN_TX_PIN = 22; +const gpio_num_t CAN_RX_PIN = 21; + +#elif defined(USE_BOARD_PICO) +#include //https://github.com/eyr1n/RP2040PIO_CAN +// 使用するマイコンに合わせてピン番号を変更してください +const uint32_t CAN_TX_PIN = 0; // 連続してなくてもいい +const uint32_t CAN_RX_PIN = 1; // GP1とGP3みたいな組み合わせでも動く + +#else +#error "ボードが選択されていません。ファイルの先頭で USE_BOARD_... のどれか1つを有効にしてください。" +#endif + +//CANライブラリよりも下で呼び出す api/HardwareCAN.hが無いって言われる +#include // DAMIAOモーター制御ライブラリ +#include // ユーティリティ関数置き場 using namespace damiao; // ============================================= // ユーザー設定項目 (User Settings) // ============================================= -const uint8_t CAN_TX_PIN = 0; -const uint8_t CAN_RX_PIN = 1; - -const uint32_t MASTER_ID = 0x00; -const uint32_t MOTOR_SLAVE_ID = 0x09; +const uint32_t MASTER_ID = 0x00; // モーターのMasterID +const uint32_t SLAVE_ID = 0x09; // モーターのSlaveID // ============================================= -Motor motor1(MASTER_ID, MOTOR_SLAVE_ID); +// モーターオブジェクトの作成 +Motor motor1(MASTER_ID, SLAVE_ID); + void setup() { Serial.begin(115200); + // シリアルモニタが起動するまで最大5秒待機 while (!Serial && millis() < 5000); - Serial.println("\n--- Utility: Read/Write Parameters Example ---"); + Serial.println("\n--- Single Motor MIT Control Example ---"); + + // 1. CAN通信の初期化 + bool can_ok = false; +#if defined(USE_BOARD_ARDUINO_R4) + can_ok = CAN.begin(CanBitRate::BR_1000k); + +#elif defined(USE_BOARD_ESP32) + can_ok = CAN.begin(CanBitRate::BR_1000k, CAN_TX_PIN, CAN_RX_PIN); - // CAN通信の初期化 +#elif defined(USE_BOARD_PICO) CAN.setTX(CAN_TX_PIN); CAN.setRX(CAN_RX_PIN); - if (!CAN.begin(CanBitRate::BR_1000k)) { + can_ok = CAN.begin(CanBitRate::BR_1000k); +#endif + + if (can_ok) { + Serial.println("CAN bus initialized successfully."); + } else { Serial.println("CAN bus initialization failed!"); - while (1); + while (1); // CAN通信が開始できない場合はここで停止 } - motor1.setCAN(&CAN); - delay(100); + motor1.setCAN(&CAN); // モーターにCANインスタンスを渡す + motor1.disable(); // 念のためモーターを無効化 Serial.println("Reading and Writing parameters..."); // ---------------------------------------------------------------- - // 1. [R/W] float型パラメータ: UV_Value (低電圧保護のしきい値) + // 1. [R/W] float型パラメータ: UV_Value (低電圧保護のしきい値) 半開区間(10.0,fmax] // ---------------------------------------------------------------- Serial.println("\n--- 1. UV_Value (float, R/W) ---"); float original_uv = motor1.param.UV_Value.get(); Serial.print("Original UV_Value: "); Serial.println(original_uv, 2); - float new_uv = 12.5f; + float new_uv = 10.1f; Serial.print("Setting UV_Value to: "); Serial.println(new_uv, 2); if (motor1.param.UV_Value.set(new_uv)) { @@ -74,7 +112,7 @@ void setup() { Serial.println(read_new_uv, 2); // ---------------------------------------------------------------- - // 2. [R/W] uint32_t型パラメータ: CTRL_MODE (制御モード) + // 2. [R/W] uint32_t型パラメータ: CTRL_MODE (制御モード) MIT = 1, POS_VEL = 2, VEL = 3, // ---------------------------------------------------------------- Serial.println("\n--- 2. CTRL_MODE (uint32_t, R/W) ---"); uint32_t original_ctrl_mode = motor1.param.CTRL_MODE.get(); @@ -139,7 +177,4 @@ void setup() { void loop() { // このサンプルではloop内で行うことは特にありません。 - // `motor.update()` を呼び出すと、CAN通信を読み続けることができます。 - // motor1.update(); - delay(1000); } From f728c33b76608f91783904bb51a6f9a2149dcca4 Mon Sep 17 00:00:00 2001 From: Suzu <47468734+Suzu-Gears@users.noreply.github.com> Date: Thu, 27 Nov 2025 16:48:05 +0900 Subject: [PATCH 2/3] fix Library name --- .../1_SingleMotor_MIT_Control/1_SingleMotor_MIT_Control.ino | 4 ++-- .../2_DoubleMotor_MIT_Control/2_DoubleMotor_MIT_Control.ino | 4 ++-- .../3_Advanced_ModeSwitching/3_Advanced_ModeSwitching.ino | 4 ++-- .../4_Utility_ReadWriteParams/4_Utility_ReadWriteParams.ino | 4 ++-- library.properties | 4 ++-- src/{DAMIAO.h => DAMIAO_Control.h} | 0 6 files changed, 10 insertions(+), 10 deletions(-) rename src/{DAMIAO.h => DAMIAO_Control.h} (100%) diff --git a/examples/1_SingleMotor_MIT_Control/1_SingleMotor_MIT_Control.ino b/examples/1_SingleMotor_MIT_Control/1_SingleMotor_MIT_Control.ino index ac8b20c..9f5448a 100644 --- a/examples/1_SingleMotor_MIT_Control/1_SingleMotor_MIT_Control.ino +++ b/examples/1_SingleMotor_MIT_Control/1_SingleMotor_MIT_Control.ino @@ -42,8 +42,8 @@ const uint32_t CAN_RX_PIN = 1; // GP1とGP3みたいな組み合わせでも動 #endif //CANライブラリよりも下で呼び出す api/HardwareCAN.hが無いって言われる -#include // DAMIAOモーター制御ライブラリ -#include // ユーティリティ関数置き場 +#include // DAMIAOモーター制御ライブラリ +#include // ユーティリティ関数置き場 using namespace damiao; diff --git a/examples/2_DoubleMotor_MIT_Control/2_DoubleMotor_MIT_Control.ino b/examples/2_DoubleMotor_MIT_Control/2_DoubleMotor_MIT_Control.ino index e4874bf..14516ec 100644 --- a/examples/2_DoubleMotor_MIT_Control/2_DoubleMotor_MIT_Control.ino +++ b/examples/2_DoubleMotor_MIT_Control/2_DoubleMotor_MIT_Control.ino @@ -40,8 +40,8 @@ const uint32_t CAN_RX_PIN = 1; // GP1とGP3みたいな組み合わせでも動 #endif //CANライブラリよりも下で呼び出す api/HardwareCAN.hが無いって言われる -#include // DAMIAOモーター制御ライブラリ -#include // ユーティリティ関数置き場 +#include // DAMIAOモーター制御ライブラリ +#include // ユーティリティ関数置き場 using namespace damiao; diff --git a/examples/3_Advanced_ModeSwitching/3_Advanced_ModeSwitching.ino b/examples/3_Advanced_ModeSwitching/3_Advanced_ModeSwitching.ino index 44dc7a0..22483b5 100644 --- a/examples/3_Advanced_ModeSwitching/3_Advanced_ModeSwitching.ino +++ b/examples/3_Advanced_ModeSwitching/3_Advanced_ModeSwitching.ino @@ -48,8 +48,8 @@ const uint32_t CAN_RX_PIN = 1; // GP1とGP3みたいな組み合わせでも動 #endif //CANライブラリよりも下で呼び出す api/HardwareCAN.hが無いって言われる -#include // DAMIAOモーター制御ライブラリ -#include // ユーティリティ関数置き場 +#include // DAMIAOモーター制御ライブラリ +#include // ユーティリティ関数置き場 using namespace damiao; diff --git a/examples/4_Utility_ReadWriteParams/4_Utility_ReadWriteParams.ino b/examples/4_Utility_ReadWriteParams/4_Utility_ReadWriteParams.ino index 076d376..e31e73c 100644 --- a/examples/4_Utility_ReadWriteParams/4_Utility_ReadWriteParams.ino +++ b/examples/4_Utility_ReadWriteParams/4_Utility_ReadWriteParams.ino @@ -43,8 +43,8 @@ const uint32_t CAN_RX_PIN = 1; // GP1とGP3みたいな組み合わせでも動 #endif //CANライブラリよりも下で呼び出す api/HardwareCAN.hが無いって言われる -#include // DAMIAOモーター制御ライブラリ -#include // ユーティリティ関数置き場 +#include // DAMIAOモーター制御ライブラリ +#include // ユーティリティ関数置き場 using namespace damiao; diff --git a/library.properties b/library.properties index aa4e9ee..807f172 100644 --- a/library.properties +++ b/library.properties @@ -1,4 +1,4 @@ -name=DAMIAO_Control_Arduino +name=DAMIAO_Control version=0.0.3 author=Ryota SUZUKI maintainer=Ryota SUZUKI @@ -7,4 +7,4 @@ paragraph=An Arduino library to control DAMIAO BLDC motors via CAN bus. It provi category=Device Control url=https://github.com/Suzu-Gears/DAMIAO_Control_Arduino architectures=* -includes=DAMIAO.h +includes=DAMIAO_Control.h diff --git a/src/DAMIAO.h b/src/DAMIAO_Control.h similarity index 100% rename from src/DAMIAO.h rename to src/DAMIAO_Control.h From f173d388d90f18b97d2fc003480853abfa08e153 Mon Sep 17 00:00:00 2001 From: Suzu <47468734+Suzu-Gears@users.noreply.github.com> Date: Thu, 27 Nov 2025 16:48:53 +0900 Subject: [PATCH 3/3] 0.0.4 --- library.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.properties b/library.properties index 807f172..0616e72 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=DAMIAO_Control -version=0.0.3 +version=0.0.4 author=Ryota SUZUKI maintainer=Ryota SUZUKI sentence=Control DAMIAO BLDC motors over CAN using Arduino HardwareCAN