Go to Qiita Advent Calendar 2024 Top
LoginSignup
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@obor1994(obor 1994)

SparkFun IoT ブラシレスモータードライバで位置制御とハプティックフィードバック

Last updated at Posted at 2023-12-20

ブラシレスモーターでハプティックフィードバックをしてロータリーエンコーダを作るSmart Knobというプロジェクトに興味があり、以下の実験用ボードを購入しました。（構成要素から考えると割高ですが、電流フィードバックまでできそうだったのでとりあえず…）

スッとできるかなと思っていたのですが、なかなかうまく動かなかったので、このボードをうまく使いたい、同じようにSmart Knobやってみたいという方向けにメモを残しておきます。

位置制御

とりあえず、角度制御をするには以下のソースコードコピペで動きます。

  • 起動してしばらくするとモーターは0°の角度に移動します。13,14のボタンを押す度に角度が90°づつ動きます。
  • 現在の角度に合わせてSTAT LEDが変化します。
  • Serial Protterを表示すると現在角度と目標角度のグラフが表示されます。

なお、ビルド前に以下のライブラリをインストールしてください。念のため動作検証したバージョンも示します。あと、HardwareはSparkFun ESP32 Thing Plus Cです。なければインストールしておいてください。

  • FastLED(3.5.0)
  • SimpleFOCDrivers(1.0.6)
#include <Wire.h>
#include <SimpleFOC.h> //http://librarymanager/All#Simple%20FOC
#include "SparkFun_TMAG5273_Arduino_Library.h"  

#include <FastLED.h>

//押しボタン
#define auxBtn2   13
#define auxBtn1   14  

//モータードライバ
#define uh16      16
#define ul17      17
#define vh18      18
#define wh19      19
#define vl23      23
#define wl33      33
#define curSense  32

//STAT LED(NeoPixel)
#define LED_COUNT     1
#define LED_PIN       2
CRGB leds[LED_COUNT];

// 角度センサ
TMAG5273 tmag; 
void initMySensorCallback();
float readMySensorCallback();
GenericSensor sensor = GenericSensor(readMySensorCallback, initMySensorCallback);

// 電流センサ
//  - shunt_resistor  - shunt resistor value
//  - gain  - current-sense op-amp gain
//  - phA   - A phase adc pin
//  - phB   - B phase adc pin
//  - phC   - C phase adc pin (optional)
InlineCurrentSense current_sense  = InlineCurrentSense(0.012, 20, 35, 36, 39);

// モータードライバ
BLDCMotor motor = BLDCMotor(7);
BLDCDriver6PWM driver = BLDCDriver6PWM(uh16, ul17, vh18, vl23, wh19, wl33, curSense);

// 目標角度
float target_angle = 0.0;

// 押しボタンコールバック
void IRAM_ATTR isr1(){
  target_angle -= M_PI/2;
}
void IRAM_ATTR isr2(){
  target_angle += M_PI/2;
}

void initMySensorCallback(){
  // 角度センサの初期化
  if(!tmag.begin(TMAG5273_I2C_ADDRESS_INITIAL, Wire) == true)
    Serial.println("angleSensor Failed");
  // 平均化モードの選択(32,16,8,4,2,1倍がある)
  tmag.setConvAvg(TMAG5273_X32_CONVERSION);
  // Choose new angle to calculate from
  tmag.setMagneticChannel(TMAG5273_X_Y_ENABLE);
  // Enable the angle calculation register
  tmag.setAngleEn(TMAG5273_XY_ANGLE_CALCULATION);
}
float readMySensorCallback(){
  // 0~2PIの間で角度を返す
  return tmag.getAngleResult()/180.0f*M_PI;
}

/////////////////////////////////////////////////////////////////////////
void setup() {
  Wire.begin();

  FastLED.addLeds<NEOPIXEL, LED_PIN>(leds, LED_COUNT); 
  leds[0] = CHSV(0,0,0);
  FastLED.show();

  //motor demo stuff
  driver.voltage_power_supply = 3.3;
  driver.pwm_frequency = 20000;
  driver.voltage_limit = 4;
  driver.init();
  current_sense.linkDriver(&driver);
  sensor.init();
  motor.linkSensor(&sensor);
  motor.linkDriver(&driver);
  motor.voltage_limit = 4;
  motor.controller = MotionControlType::angle;//MotionControlType::velocity_openloop;

  // controller configuration based on the control type 
  // velocity PI controller parameters
  // default P=0.5 I = 10
  motor.PID_velocity.P = 0.2;
  motor.PID_velocity.I = 0.05; //0.1;
  motor.PID_velocity.D = 0.003;

  // jerk control using voltage voltage ramp
  // default value is 300 volts per sec  ~ 0.3V per millisecond
  motor.PID_velocity.output_ramp = 1000;
  
  // velocity low pass filtering
  // default 5ms - try different values to see what is the best. 
  // the lower the less filtered
  motor.LPF_velocity.Tf = 0.1;

  // angle P controller 
  // default P=20
  motor.P_angle.P = 20;
  //motor.P_angle.I = 0;
  //motor.P_angle.D = 0.01;
  
  // angle low pass filtering
  // default 0 - disabled  
  // use only for very noisy position sensors - try to avoid and keep the values very small
  motor.LPF_angle.Tf = 0; // default 0

  //  maximal velocity of the position control
  // default 20
  motor.velocity_limit = 20;

  motor.init();
  current_sense.init();
  motor.linkCurrentSense(&current_sense);

  motor.initFOC();
  //motor.disable();

  pinMode(auxBtn1, INPUT_PULLUP); // Sets pin 14 on the ESP32 as an interrupt
  attachInterrupt(auxBtn1, isr1, FALLING); // Triggers when aux1 is pulled to GND (button pressed)
  pinMode(auxBtn2, INPUT_PULLUP); // Sets pin 13 on the ESP32 as an interrupt
  attachInterrupt(auxBtn2, isr2, FALLING); // Triggers when aux2 is pulled to GND (button pressed)

  Serial.begin(115200);
}

/////////////////////////////////////////////////////////////////////////
void loop() { 
  static int cnt = 0;
  
  cnt++;
  if(cnt>=10){
    cnt = 0;
    Serial.print(motor.shaft_angle_sp);
    Serial.print(" ");
    Serial.print(motor.shaft_angle);
    //Serial.print(" ");
    //Serial.print(motor.shaft_velocity_sp);
    //Serial.print(" ");
    //Serial.print(motor.shaft_velocity);
    Serial.println("");
    
    //角度に合わせてLEDの色を変更
    leds[0] = CHSV(motor.shaft_angle/(2*M_PI)*255,255,50);
    FastLED.show();
  }

  motor.loopFOC();
  motor.move(target_angle);
}

一応このコードでボードに乗っているすべての機能を試せます。
なお、インライン電流センサを有効にし、電流フィードバックをかけていますが、これがどの程度利いてるのかはよく分かっていません。

PIDパラメータをもっと攻めたい人はWebUIで動作確認しながら調整できるらしいのですが、ちょっと試した感じうまくうごかせませんでした。誰かうまく動かせたら教えてください。
https://docs.simplefoc.com/webcontroller

角度センサのライブラリ修正

さて、ここからがミソです。上のコードを動かして、指でモーターに外乱を加えると目標角度に戻ろうと制御がかかります。しかし、特定の角度でガタガタすることに気が付くはずです。このガタガタは触覚フィードバックにおいて致命的です。

これはライブラリの角度レジスタ読み出しのコードがマズいためです。詳しく言うと、角度は2つのレジスタの値を組み合わせて得ているのですが、これらを別々のタイミングで読みだしているためそれぞれ違うサンプリングの値になることがあるのです。

ということで、Arduino/libraries/SparkFun_TMAG5273_Arduino_Library/src/SparkFun_TMAG5273_Arduino_Library.cppを修正します。L2637付近のgetAngleResult()の冒頭部分を以下の要領で修正してください。

// この2行を下の4行に変更
//angleLSB = readRegister(TMAG5273_REG_ANGLE_RESULT_LSB) & 0b11111111;
//angleMSB = readRegister(TMAG5273_REG_ANGLE_RESULT_MSB);
uint8_t regVal[2];
readRegisters(TMAG5273_REG_ANGLE_RESULT_MSB, regVal, 2);
angleLSB = regVal[1];
angleMSB = regVal[0];

ライブラリがgitにあるので、そこにIssueとして投げたら良いと思うのですが、そのあたりのマナーをよくわかってないので、とりあえず自分のだけ修正して満足しています。誰か余力あったら修正要望出してください…

SmartKnobを作る

では最後に角度に合わせて制御角度を変更して、ハプティックフィードバックをやってみます。以下のコードでとりあえず試せます。

  • ボタン13,14でモードを切り替えます。（スムーズ、6ポジション（触覚強め、弱め）、12ポジション、100ポジション、100ポジション（5ポジションごとに触覚強め））
  • モードにあわせてLEDの色がかわります。
#include <Wire.h>
#include <SimpleFOC.h> //http://librarymanager/All#Simple%20FOC
#include "SparkFun_TMAG5273_Arduino_Library.h"  

#include <FastLED.h>

//押しボタン
#define auxBtn2   13
#define auxBtn1   14  

//モータードライバ
#define uh16      16
#define ul17      17
#define vh18      18
#define wh19      19
#define vl23      23
#define wl33      33
#define curSense  32

//STAT LED(NeoPixel)
#define LED_COUNT     1
#define LED_PIN       2
CRGB leds[LED_COUNT];

// 角度センサ
TMAG5273 tmag; 
void initMySensorCallback();
float readMySensorCallback();
GenericSensor sensor = GenericSensor(readMySensorCallback, initMySensorCallback);

// 電流センサ
//  - shunt_resistor  - shunt resistor value
//  - gain  - current-sense op-amp gain
//  - phA   - A phase adc pin
//  - phB   - B phase adc pin
//  - phC   - C phase adc pin (optional)
InlineCurrentSense current_sense  = InlineCurrentSense(0.012, 20, 35, 36, 39);

// モータードライバ
BLDCMotor motor = BLDCMotor(7);
BLDCDriver6PWM driver = BLDCDriver6PWM(uh16, ul17, vh18, vl23, wh19, wl33, curSense);

float target_angle = 0.0; // 目標角度
int rotaryMode = 0; // モード
int modeCnt = 6; // モード数

// 押しボタンコールバック
void IRAM_ATTR isr1(){
  rotaryMode++;
  if(rotaryMode>=modeCnt){
    rotaryMode=modeCnt-1;
  }
}
void IRAM_ATTR isr2(){
  rotaryMode--;
  if(rotaryMode<0){
    rotaryMode=0;
  }
}

void initMySensorCallback(){
  // 角度センサの初期化
  if(!tmag.begin(TMAG5273_I2C_ADDRESS_INITIAL, Wire) == true)
    Serial.println("angleSensor Failed");
  // 平均化モードの選択(32,16,8,4,2,1倍がある)
  tmag.setConvAvg(TMAG5273_X32_CONVERSION);
  // Choose new angle to calculate from
  tmag.setMagneticChannel(TMAG5273_X_Y_ENABLE);
  // Enable the angle calculation register
  tmag.setAngleEn(TMAG5273_XY_ANGLE_CALCULATION);
}
float readMySensorCallback(){
  // 0~2PIの間で角度を返す
  return tmag.getAngleResult()/180.0f*M_PI;
}

/////////////////////////////////////////////////////////////////////////
void setup() {
  Wire.begin();

  FastLED.addLeds<NEOPIXEL, LED_PIN>(leds, LED_COUNT); 
  leds[0] = CHSV(0,255,0);
  FastLED.show();

  //motor demo stuff
  driver.voltage_power_supply = 3.3;
  driver.pwm_frequency = 20000;
  driver.voltage_limit = 4;
  driver.init();
  current_sense.linkDriver(&driver);
  sensor.init();
  motor.linkSensor(&sensor);
  motor.linkDriver(&driver);
  motor.voltage_limit = 4;
  motor.controller = MotionControlType::angle;//MotionControlType::velocity_openloop;

  // controller configuration based on the control type 
  // velocity PI controller parameters
  // default P=0.5 I = 10
  motor.PID_velocity.P = 0.2;
  motor.PID_velocity.I = 0; //0.1;
  motor.PID_velocity.D = 0.003;

  // jerk control using voltage voltage ramp
  // default value is 300 volts per sec  ~ 0.3V per millisecond
  motor.PID_velocity.output_ramp = 1000;
  
  // velocity low pass filtering
  // default 5ms - try different values to see what is the best. 
  // the lower the less filtered
  motor.LPF_velocity.Tf = 0.1;

  // angle P controller 
  // default P=20
  motor.P_angle.P = 20;
  //motor.P_angle.I = 5;
  //motor.P_angle.D = 0.01;
  
  // angle low pass filtering
  // default 0 - disabled  
  // use only for very noisy position sensors - try to avoid and keep the values very small
  motor.LPF_angle.Tf = 0.02; // default 0

  //  maximal velocity of the position control
  // default 20
  motor.velocity_limit = 20;

  motor.init();
  current_sense.init();
  motor.linkCurrentSense(&current_sense);

  motor.initFOC();
  //motor.disable();

  pinMode(auxBtn1, INPUT_PULLUP); // Sets pin 14 on the ESP32 as an interrupt
  attachInterrupt(auxBtn1, isr1, FALLING); // Triggers when aux1 is pulled to GND (button pressed)
  pinMode(auxBtn2, INPUT_PULLUP); // Sets pin 13 on the ESP32 as an interrupt
  attachInterrupt(auxBtn2, isr2, FALLING); // Triggers when aux2 is pulled to GND (button pressed)
  delay(100);

  Serial.begin(115200);
}

/////////////////////////////////////////////////////////////////////////
void loop() { 
  static int cnt = 0;
  static int prevMode = -1;
  static int stepCnt = 0;
  
  cnt++;
  if(cnt>=10){
    cnt = 0;
    Serial.print(stepCnt);
    //Serial.print(motor.shaft_angle_sp);
    //Serial.print(" ");
    //Serial.print(motor.shaft_angle);
    //Serial.print(" ");
    //Serial.print(motor.shaft_velocity_sp);
    //Serial.print(" ");
    //Serial.print(motor.shaft_velocity);
    Serial.println("");
  }

  // open loop velocity movement
  // using motor.voltage_limit and motor.velocity_limit
  // Basic motor movement
  motor.loopFOC();

  float steps;
  if(prevMode!= rotaryMode){
    prevMode = rotaryMode;
    leds[0] = CHSV(rotaryMode*(255/modeCnt),255,50);
    FastLED.show();
  }
  switch(rotaryMode){
    case 0:
      motor.PID_velocity.P = 0;
      steps = 0;
      break; 
    case 1:
      motor.PID_velocity.P = 0.1;
      steps = 6;
      break; 
    case 2:
      motor.PID_velocity.P = 0.3;
      steps = 6;
      break; 
    case 3:
      motor.PID_velocity.P = 0.3;
      steps = 12;
      break; 
    case 4:
      motor.PID_velocity.P = 0.3;
      steps = 100;
      break; 
    case 5:
      if(stepCnt%5==0)
        motor.PID_velocity.P = 0.5;
      else        
        motor.PID_velocity.P = 0.2;
      steps = 100;
      break; 
  }
  stepCnt = round(motor.shaft_angle / (M_PI * 2.0f / steps));
  if(steps>0)target_angle =  stepCnt * (M_PI * 2.0f / steps);
  motor.move(target_angle);

}

試してみると、触り心地がなんだかノイジーです。角度センサの平均化を低くするとひどくなりますので…多分角度センサの誤差なんだと思います。
もっといい角度センサ使ってリトライしてみたいですね。磁気エンコーダってこんなものなのかなぁ…。

0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?