Arduino_Unoでロボマスモーター回してみた！

概要

2022年大会以降ROBOMASTERのモーターが注目を浴びるようになった。
この記事はそんなロボマスモーターをArduino Unoを使って動かす記事です。

はじめに

NHK学生ロボコン2022年大会において、ROBOMASTERモーターを採用している大学がありました。(豊橋，東大)
モーター自体のスペック、重量、大きさ、どれも申し分ないということで、一気に注目を浴びました。(この大会の半年後、ロボコニスト御用達の朱雀技研様が閉店なさった背景も相まって)
しかし、予てよりブラシレスモーターの制御は難しいと言われており、ロボマスモーターはCAN通信で制御できると聞いても「？？？」で混乱する人が多く、「CANCAN」とキャンキャン鳴く人が続出しました。(観測者：岩根)
この記事はそんな犬と猫のなり損ないのような声で鳴く人々を救おうとした記事です。

準備するもの

ロボマスモーター
M2006：URL：https://store.dji.com/jp/product/rm-m2006-p36-brushless-motor?vid=40031
M3508：URL：https://store.dji.com/jp/product/rm-m3508-p19-brushless-dc-gear-motor?vid=32501
ロボマス回路
C610：URL：https://store.dji.com/jp/product/rm-c610-brushless-dc-motor-speed-control?vid=40021
C620：URL：https://store.dji.com/jp/product/rm-c620-brushless-dc-motor-speed-controller?vid=32491
マイコン周り
Arduino Uno
canモジュール:URL：https://www.amazon.co.jp/s?k=MCP2515

回路_配線

can_moduleのVCCピンをArduino Unoの5Vピンに接続しましょう。
can_moduleのGNDピンをArduino UnoのGNDピンに接続しましょう。
can_moduleのCSピンをArduino Unoの10ピンに接続しましょう。
can_moduleのSOピンをArduino Unoの12ピンに接続しましょう。
can_moduleのSIピンをArduino Unoの11ピンに接続しましょう。
can_moduleのSCKピンをArduino Unoの13ピンに接続しましょう。
※can_moduleの抵抗のジャンパーピンを短絡させましょう。
　短絡させないとcan通信できません。忘れないように。

ライブラリの導入

https://github.com/autowp/arduino-mcp2515
ここから、zipファイルをダウンロードして、Arduinoライブラリに追加してください。

コード

#include <SPI.h>
#include <mcp2515.h>

// --- 定数定義 ---
#define M2006_GEAR_RATIO 36.0 // ギア比36
#define M3508_GEAR_RATIO 19.0 // ギア比19

// --- グローバル変数 ---
struct can_frame canMsgSend;
struct can_frame canMsgReceive;

MCP2515 mcp2515(8); // CSピンを8番に設定

short current = 0;// モーターに送る電流値 (M2006:-10000 〜 10000)
float rotation = 0; // モーターの角度 [deg]
short rpm = 0; // モーターの現在RPM
short torque = 0; // モーターの現在トルク
short temperature = 0; // モーターの温度[℃]
float target = 0; // モーターの目標RPM
const float pi = 3.141592;
static float iterm = 0;
static float prediff = 0;
static float pid_output = 0; // PID制御の出力
static float prev_current = 0; // 前回のモーター指令値
volatile float smoothed_rpm = 0.0; // ★ LPF適用後の滑らかなRPM

// *** 安全対策用の変数 (タイマー割り込み方式) ***
// volatileキーワードは、割り込みの内外で変数を安全に扱うためのおまじないです
volatile unsigned int can_timeout_counter = 0;
// タイムアウトカウンタの上限値 (200ms / 35ms　≒ 5.7 なので、余裕をもって 6 に設定)
const unsigned int CAN_TIMEOUT_TICKS = 6;
volatile bool motor_connected = false; // モーターとの接続状態フラグ

// --- 関数 ---

// CANメッセージを送信する関数
void sendValues(){
  canMsgSend.data[0] = current >> 8 & 0xff;
  canMsgSend.data[1] = current & 0xff;
  mcp2515.sendMessage(&canMsgSend);
}

// PID制御の状態をリセットする関数
void reset_pid_control(){
 target = 0;
 current = 0;
 iterm = 0;
 prediff = 0;
 sendValues(); // 安全のため停止コマンド(current=0)を送信
}

// CANメッセージ受信時の割り込みサービスルーチン
void canISR(){
  if(mcp2515.readMessage(&canMsgReceive) == MCP2515::ERROR_OK){
    // モーターID 0x201 からのフィードバックか確認
  if(canMsgReceive.can_id == 0x201){
// rotation
 unsigned short r = (canMsgReceive.data[0] << 8) | (canMsgReceive.data[1] & 0xff);
 rotation = (float)r/ 8192 * 360; // 0-8191が360度に対応

// speed(RPM)
rpm = (canMsgReceive.data[2] << 8) | (canMsgReceive.data[3] & 0xff);
// ★★★ ローパスフィルタ（平滑化）★★★
      // 90%は古い値、10%は新しい値で更新する (係数は調整可能)
      smoothed_rpm = (0.9 * smoothed_rpm) + (0.1 * (float)rpm);

// torque (トルク)
torque = (canMsgReceive.data[4] << 8) | (canMsgReceive.data[5] & 0xff);

// temperature (温度)
temperature = canMsgReceive.data[6];

 
   }
   
}
}


// タイマー割り込みによるPID制御
ISR(TIMER1_COMPA_vect)
{
  float diff = (smoothed_rpm - target); // 偏差 (現在値 -　目標値)
  float dt = 0.025; // 割り込み周期 (1 / (16MHz / 256 / 2187.5))

  // I項 (積分)
  iterm += diff *dt;
  const float iterm_limit = 140000.0;

  // D項 (微分)
  float differencial = (prediff - diff) /dt;
  prediff = diff;

  // PIDゲイン (これらの値は実機の動作を見ながら調整が必要です)
  float Kf = 0.80; // フィードフォワードゲイン (目標値に直接掛かる)
  float Kp = 0.1; // 比例ゲイン
  float Ki = 0.04; // 積分ゲイン
  const float Kd = 0.001; //微分ゲイン

  // PIDF制御量の計算
  float value = Kf * target - Kp * diff - Ki * iterm - Kd * differencial;
  
  // 電流値に変換して代入
  current = (short)constrain(value, -20000, 20000); // 電流値の上限下限
  
  // 目標値と現在値がほぼ0のとき、I項を減衰させてオーバーシュートを防ぐ
  if (abs(target) < 1 && abs(smoothed_rpm) < 1) {
    iterm *= 0.5;
  } 
  sendValues(); // 計算した電流値をモーターに送信
  //if (target == 0 && rpm == 0)iterm *= 0.5;

  prev_current = current; // 次の周期のために現在の電流値を保存

  

}
void setup(){
 Serial.begin(115200);
 Serial.println("シリアル制御スタート。目標RPMを入力してください　(例:1000)");
 
  SPI.begin();
  canMsgSend.can_id  = 0x200; 
  canMsgSend.can_dlc = 8;
  canMsgSend.data[0] = 0;
  canMsgSend.data[1] = 0;
  canMsgSend.data[2] = 0;
  canMsgSend.data[3] = 0;
  canMsgSend.data[4] = 0;
  canMsgSend.data[5] = 0;
  canMsgSend.data[6] = 0;
  canMsgSend.data[7] = 0;
   mcp2515.reset();
    mcp2515.setBitrate(CAN_1000KBPS, MCP_8MHZ);
    mcp2515.setNormalMode();
  // mcp2515.reset();
  // mcp2515.setBitrate(CAN_1000KBPS, MCP_8MHZ);

   pinMode(2, INPUT_PULLUP);
   attachInterrupt(digitalPinToInterrupt(2), canISR, FALLING);

  cli();
  TCCR1A  = 0;
  TCCR1B  = 0;
  TCCR1B |= (1 << WGM12) | (1 << CS12);  //CTCmode //prescaler to 256
  OCR1A   = 1562.5 - 1;
  TIMSK1 |= (1 << OCIE1A);
  sei();
}
 
// メインループ
void loop() {
 
  if (Serial.available() > 0) {
  float new_target = Serial.parseFloat();
  
  // M3508の最大RPMリミット
  const float MAX_MOTOR_RPM = 18000.0;
  target = constrain(new_target, -MAX_MOTOR_RPM, MAX_MOTOR_RPM);
  
  Serial.print("Target RPM set to:");
  Serial.println(target);
  
  while (Serial.available() > 0) {
   Serial.read();
   }
  }

  Serial.print("Target: ");
  Serial.print(target);
  Serial.print("\t Current RPM: ");
  Serial.println(rpm); // CANで受信した現在のRPM

  delay(50);
  
 }

コンパイル後、シリアルモニタを開いて、テキトーに数字を入れてください。動きます。