MENU

デジタル電流制御

デジタル制御

デジタル制御についての備忘録としてここに記します。

制御プラント

以下の単純な単相ハーフブリッジインバータを制御プラントにして, デジタル制御の議論を行います。

Simple converter

デジタル制御を実現するにはマイクロコントローラを使用します.

Microcontrollers (MCU)

デジタル制御を実現するための組み込みシステムについて

・CPU, RAM, ROM

・A/D, PWM, Timer

を必要とします。

デジタル電流制御

以下の3点に注意します。

・A/Dコンバータを用いた電流検出

サンプリング/変換の時間遅れ

量子化ビットエラー

・CPU内での計算

実行時間の遅延

丸め誤差

PWM制御

キャリア信号/基準値の分解能

これらの要件をクリアするMCUが制御に必要になります。

ダブルアップデートでのタイミングチャート

以下にPWMキャリアの山と谷をサンプリングしたダブルアップデートの模式図を示します。

Timing Chart in Double Update

PWMキャリアの山の部分と谷の部分で電流のサンプリングを行い, 各サンプリング間にAD変換と演算を行い, サンプリングして次のサンプリング点にてPWM制御演算を更新することによって制御を実現します。更新タイミングの遅れ時間は, T_D = T_S = T_{sw}/2 となります。

また山と谷のサンプリングではなく, 山のみのサンプリングを行った際の遅れ時間は, T_D = T_S = T_{sw} となります。

デジタル電流制御

連続時間領域における制御ブロック図を示します。

Digital Current Control

K : 伝達関数内の電流フィードバックゲイン

 e^{-sT} : 遅れ時間

H(t) : ゼロ次ホールド関数

次にデジタル時間領域におけるパルス伝達関数のブロック図を示します。

Pulse Transfer Function

K : パルス伝達関数内の電流フィードバックゲイン

z^{-1} : サンプリング遅れ時間

{ \displaystyle
G^{*}(z) = Kz^{-1} \frac{T_s/L}{z-1}
}

パルス伝達関数

オープンループパルス伝達関数

{ \displaystyle
G^{*}(z) = Kz^{-1} \frac{T_s/L}{z-1}
}

・クローズドループパルス伝達関数

{ \displaystyle
W^{*}(z) = \frac{G^{*}(z)}{1+G^{*}(z)} = \frac{\frac{KT_s}{L}}{z^2-z+\frac{KT_s}{L}}
}

・極 z を求めると,

{ \displaystyle
z = \frac{1}{2} ± \sqrt{\frac{1}{4}-\frac{K T_s}{L}}
}

となります。

z 平面での安定性

{ \displaystyle
z = e^{\sigma±j\omega}
}

s平面下では極 \sigma は収束, 発散を. \omega は振動性を表しています。

\sigma は負に配置されれば収束し, \omega が0であれば振動はなくなります。

Stability Analysis on z plane

z平面下では, e^{\sigma} は収束性を, e^{\omega} は振動性を示します。

e^{\sigma} が単位円内にあれば収束し, e^{\omega} の角度が0であれば振動はなくなります。

パルス伝達関数

ここで, 極の議論に戻ります。

{ \displaystyle
z = \frac{1}{2} ± \sqrt{\frac{1}{4}-\frac{K T_s}{L}}
}

・安定限界ゲイン

{ \displaystyle
K = \frac{L}{T_s}
}

このとき,

{ \displaystyle
z = \frac{1}{2} ± j \frac{\sqrt{3}}{2}
}

となり, 安定限界に極配置されます。

・クリティカルゲイン

{ \displaystyle
K = \frac{L}{4 T_s}
}

のとき,

{ \displaystyle
z = \frac{1}{2}
}

となり, 臨界制動の挙動をとります。

z plane

時間領域における応答

では時間領域における応答を見てみましょう. python control を使用して図示します.

"""
 time domain response of digital control
"""
import math
import matplotlib.pyplot as plt
import numpy as np
from control import tf, c2d
from control.matlab import step

### 数式     
s = tf('s')  
z = tf('z')  
L = 1        # インダクタンス
ts = 0.2     # サンプリング時間

# ゲイン
Kc = L/(4*ts)
Kh = L/(2*ts)
Ks = L/ts

# クローズドループ伝達関数
Wc = (Kc*ts/L)/(z*z - z + Kc*ts/L)
Wh = (Kh*ts/L)/(z*z - z + Kh*ts/L)
Ws = (Ks*ts/L)/(z*z - z + Ks*ts/L)

print(Wc)
print(Wh)
print(Ws)

# 0次ホールドによる離散化
print('discrete time system(zoh)', Wc)
print('discrete time system (zoh) ', Wh)
print('discrete time system(zoh)', Ws)

### 図示
fig, ax = plt.subplots(1, 3, figsize = (6, 2.6))

#離散時間システム(0次ホールドによる離散化)
T = np.arange(0, 4, ts)

# Kc = L/4Ts
y, t = step(Wc, T)
ax[0].plot(t, y, ls = '-', marker = 'o', label = 'Kc = L/4Ts')
# Kh = L/2Ts
y, t = step(Wh, T)
ax[1].plot(t, y, ls = '-', marker = 'o', label = 'Kh = L/2Ts')
# Ks = L/Ts
y, t = step(Ws, T)
ax[2].plot(t, y, ls = '-', marker = 'o', label = 'Ks = L/Ts')

title = ['Kc', 'Kh', 'Ks']
for i in range(3):
    ax[i].set_xlabel('t')
    ax[i].set_ylabel('y')
    ax[i].set_title(title[i])
    ax[i].legend()
fig.tight_layout()
plt.show()

Time domain response

 K = \frac{L}{4Ts} のとき, クリティカルゲインとなり振動なく収束します。

 K = \frac{L}{2Ts} のとき, クリティカルゲイン時より早く収束しますが振動成分が残ります。

 K = \frac{L}{Ts} のとき, 収束はさらに早くなるもののかなり振動の大きい応答になります。

デッドビート制御

Dead Beat Current Control

・遅延要素

MCU内のサンプリング遅延

電流応答の遅れ(インダクタンスの要素)

少なくとも 2サンプリングは電流応答の遅れがある 

デッドビート制御の伝達関数

・2サンプリング遅延のクローズドループ伝達関数であると仮定します

{ \displaystyle
W^{*}(z) = z^{-2}
}

オープンループ伝達関数

{ \displaystyle
G^{*}(z) = G^{*}_c(z) z^{-1} \frac{T_s/L}{z-1}
}

・クローズドループ伝達関数

{ \displaystyle
W^{*}(z) = \frac{G^{*}_c(z) z^{-1}T_s}{L(z-1)+G^{*}_c(z)z^{-1}T_s} = z^{-2}
}

G^{*}_c(z) から得られる伝達関数の結果

{ \displaystyle
G^{*}_c (z) = \frac{L}{T_s} \frac{z}{z+1}
}

・極

{ \displaystyle
z = 0
}

・実装

{ \displaystyle
G^{*}_c (z) = \frac{Y^{*}(z)}{X^{*}(z)} = \frac{L}{T_s}\frac{z}{z+1}
}
{ \displaystyle
(z+1)Y^{*}(z) = \frac{L}{T_s}zX^{*}(z)
}
{ \displaystyle
Y^{*}(z) = \frac{L}{T_s}X^{*}(z)-z^{-1}Y^{*}(z)
}

・時間領域表現

{ \displaystyle
y(n)=\frac{L}{T_s}x(n)-y(n-1)
}

ブロック図

Dead Beat Control

1サンプリング遅延と加算器で表現されます.

時間応答

では時間応答を見てみましょう。

"""
 time domain response of digital control
"""
import math
import matplotlib.pyplot as plt
import numpy as np
from control import tf, c2d
from control.matlab import step

### 数式     
s = tf('s')  
z = tf('z')  
L = 1        # インダクタンス
ts = 0.2     # サンプリング時間

# ゲイン
Kd = L/ts
# デッドビートコントローラ
Gz = Kd * z / (z + 1)
# オープンループ伝達関数
Goz = Gz * (1/z) * ((ts/L)/(z-1))
# クローズドループ伝達関数
Wz = Goz / (1+Goz)
print(Wz)

# 0次ホールドによる離散化
print('discrete time system (zoh) ', Wz)

### 図示
fig, ax = plt.subplots(1, 2, figsize = (6, 2.6))
# 連続時間システム
Tc = np.arange(0, 4, 0.01)
Uc = 1
y, t = step(Uc, Tc)
ax[0].plot(t, y, ls = '-')

#離散時間システム(0次ホールドによる離散化)
T = np.arange(0, 4, ts)

y, t = step(Wz, T)
ax[0].plot(t, y, ls = '-', marker = 'o', label = 'Dead Beat')


title = ['Dead Beat']
for i in range(1):
    ax[i].set_xlabel('t')
    ax[i].set_ylabel('y')
    ax[i].set_title(title[i])
    ax[i].legend()
fig.tight_layout()
plt.show()

Dead Beat Control

2サンプリングで収束することがわかります。

デジタル電流制御②

前回に続いて, 今回は電流制御の応用についての議論を行います.

デッドビート制御

f:id:Manao55:20211116214152j:plain
Dead Beat Current Control

・遅延要素

MCU内のサンプリング遅延

電流応答の遅れ(インダクタンスの要素)

少なくとも 2サンプリングは電流応答の遅れがある 

デッドビート制御の伝達関数

・2サンプリング遅延のクローズドループ伝達関数であると仮定します

{ \displaystyle
W^{*}(z) = z^{-2}
}

オープンループ伝達関数

{ \displaystyle
G^{*}(z) = G^{*}_c(z) z^{-1} \frac{T_s/L}{z-1}
}

・クローズドループ伝達関数

{ \displaystyle
W^{*}(z) = \frac{G^{*}_c(z) z^{-1}T_s}{L(z-1)+G^{*}_c(z)z^{-1}T_s} = z^{-2}
}

G^{*}_c(z) から得られる伝達関数の結果

{ \displaystyle
G^{*}_c (z) = \frac{L}{T_s} \frac{z}{z+1}
}

・極

{ \displaystyle
z = 0
}

・実装

{ \displaystyle
G^{*}_c (z) = \frac{Y^{*}(z)}{X^{*}(z)} = \frac{L}{T_s}\frac{z}{z+1}
}
{ \displaystyle
(z+1)Y^{*}(z) = \frac{L}{T_s}zX^{*}(z)
}
{ \displaystyle
Y^{*}(z) = \frac{L}{T_s}X^{*}(z)-z^{-1}Y^{*}(z)
}

・時間領域表現

{ \displaystyle
y(n)=\frac{L}{T_s}x(n)-y(n-1)
}

ブロック図

f:id:Manao55:20211117102036j:plain
Dead Beat Control

1サンプリング遅延と加算器で表現されます.

時間応答

では時間応答を見てみましょう。

"""
 time domain response of digital control
"""
import math
import matplotlib.pyplot as plt
import numpy as np
from control import tf, c2d
from control.matlab import step

### 数式     
s = tf('s')  
z = tf('z')  
L = 1        # インダクタンス
ts = 0.2     # サンプリング時間

# ゲイン
Kd = L/ts
# デッドビートコントローラ
Gz = Kd * z / (z + 1)
# オープンループ伝達関数
Goz = Gz * (1/z) * ((ts/L)/(z-1))
# クローズドループ伝達関数
Wz = Goz / (1+Goz)
print(Wz)

# 0次ホールドによる離散化
print('discrete time system (zoh) ', Wz)

### 図示
fig, ax = plt.subplots(1, 2, figsize = (6, 2.6))
# 連続時間システム
Tc = np.arange(0, 4, 0.01)
Uc = 1
y, t = step(Uc, Tc)
ax[0].plot(t, y, ls = '-')

#離散時間システム(0次ホールドによる離散化)
T = np.arange(0, 4, ts)

y, t = step(Wz, T)
ax[0].plot(t, y, ls = '-', marker = 'o', label = 'Dead Beat')


title = ['Dead Beat']
for i in range(1):
    ax[i].set_xlabel('t')
    ax[i].set_ylabel('y')
    ax[i].set_title(title[i])
    ax[i].legend()
fig.tight_layout()
plt.show()

f:id:Manao55:20211120000327j:plain
Dead Beat Control

2サンプリングで収束することがわかります。

デジタル電流制御

デジタル制御①

デジタル制御についての備忘録としてここに記します。

制御プラント

以下の単純な単相ハーフブリッジインバータを制御プラントにして, デジタル制御の議論を行います。

Simple converter

デジタル制御を実現するにはマイクロコントローラを使用します.

Microcontrollers (MCU)

デジタル制御を実現するための組み込みシステムについて

・CPU, RAM, ROM

・A/D, PWM, Timer

を必要とします。

デジタル電流制御

以下の3点に注意します。

・A/Dコンバータを用いた電流検出

サンプリング/変換の時間遅れ

量子化ビットエラー

・CPU内での計算

実行時間の遅延

丸め誤差

PWM制御

キャリア信号/基準値の分解能

これらの要件をクリアするMCUが制御に必要になります。

ダブルアップデートでのタイミングチャート

以下にPWMキャリアの山と谷をサンプリングしたダブルアップデートの模式図を示します。

Timing Chart in Double Update

PWMキャリアの山の部分と谷の部分で電流のサンプリングを行い, 各サンプリング間にAD変換と演算を行い, サンプリングして次のサンプリング点にてPWM制御演算を更新することによって制御を実現します。更新タイミングの遅れ時間は, T_D = T_S = T_{sw}/2 となります。

また山と谷のサンプリングではなく, 山のみのサンプリングを行った際の遅れ時間は, T_D = T_S = T_{sw} となります。

デジタル電流制御

連続時間領域における制御ブロック図を示します。

Digital Current Control

K : 伝達関数内の電流フィードバックゲイン

 e^{-sT} : 遅れ時間

H(t) : ゼロ次ホールド関数

次にデジタル時間領域におけるパルス伝達関数のブロック図を示します。

Pulse Transfer Function

K : パルス伝達関数内の電流フィードバックゲイン

z^{-1} : サンプリング遅れ時間

{ \displaystyle
G^{*}(z) = Kz^{-1} \frac{T_s/L}{z-1}
}

パルス伝達関数

オープンループパルス伝達関数

{ \displaystyle
G^{*}(z) = Kz^{-1} \frac{T_s/L}{z-1}
}

・クローズドループパルス伝達関数

{ \displaystyle
W^{*}(z) = \frac{G^{*}(z)}{1+G^{*}(z)} = \frac{\frac{KT_s}{L}}{z^2-z+\frac{KT_s}{L}}
}

・極 z を求めると,

{ \displaystyle
z = \frac{1}{2} ± \sqrt{\frac{1}{4}-\frac{K T_s}{L}}
}

となります。

z 平面での安定性

{ \displaystyle
z = e^{\sigma±j\omega}
}

s平面下では極 \sigma は収束, 発散を. \omega は振動性を表しています。

\sigma は負に配置されれば収束し, \omega が0であれば振動はなくなります。

Stability Analysis on z plane

z平面下では, e^{\sigma} は収束性を, e^{\omega} は振動性を示します。

e^{\sigma} が単位円内にあれば収束し, e^{\omega} の角度が0であれば振動はなくなります。

パルス伝達関数

ここで, 極の議論に戻ります。

{ \displaystyle
z = \frac{1}{2} ± \sqrt{\frac{1}{4}-\frac{K T_s}{L}}
}

・安定限界ゲイン

{ \displaystyle
K = \frac{L}{T_s}
}

このとき,

{ \displaystyle
z = \frac{1}{2} ± j \frac{\sqrt{3}}{2}
}

となり, 安定限界に極配置されます。

・クリティカルゲイン

{ \displaystyle
K = \frac{L}{4 T_s}
}

のとき,

{ \displaystyle
z = \frac{1}{2}
}

となり, 臨界制動の挙動をとります。

z plane

時間領域における応答

では時間領域における応答を見てみましょう. python control を使用して図示します.

"""
 time domain response of digital control
"""
import math
import matplotlib.pyplot as plt
import numpy as np
from control import tf, c2d
from control.matlab import step

### 数式     
s = tf('s')  
z = tf('z')  
L = 1        # インダクタンス
ts = 0.2     # サンプリング時間

# ゲイン
Kc = L/(4*ts)
Kh = L/(2*ts)
Ks = L/ts

# クローズドループ伝達関数
Wc = (Kc*ts/L)/(z*z - z + Kc*ts/L)
Wh = (Kh*ts/L)/(z*z - z + Kh*ts/L)
Ws = (Ks*ts/L)/(z*z - z + Ks*ts/L)

print(Wc)
print(Wh)
print(Ws)

# 0次ホールドによる離散化
print('discrete time system(zoh)', Wc)
print('discrete time system (zoh) ', Wh)
print('discrete time system(zoh)', Ws)

### 図示
fig, ax = plt.subplots(1, 3, figsize = (6, 2.6))

#離散時間システム(0次ホールドによる離散化)
T = np.arange(0, 4, ts)

# Kc = L/4Ts
y, t = step(Wc, T)
ax[0].plot(t, y, ls = '-', marker = 'o', label = 'Kc = L/4Ts')
# Kh = L/2Ts
y, t = step(Wh, T)
ax[1].plot(t, y, ls = '-', marker = 'o', label = 'Kh = L/2Ts')
# Ks = L/Ts
y, t = step(Ws, T)
ax[2].plot(t, y, ls = '-', marker = 'o', label = 'Ks = L/Ts')

title = ['Kc', 'Kh', 'Ks']
for i in range(3):
    ax[i].set_xlabel('t')
    ax[i].set_ylabel('y')
    ax[i].set_title(title[i])
    ax[i].legend()
fig.tight_layout()
plt.show()

Time domain response

 K = \frac{L}{4Ts} のとき, クリティカルゲインとなり振動なく収束します。

 K = \frac{L}{2Ts} のとき, クリティカルゲイン時より早く収束しますが振動成分が残ります。

 K = \frac{L}{Ts} のとき, 収束はさらに早くなるもののかなり振動の大きい応答になります。

TB6605FTG BLDC motor shield によるBLDC Motor 速度制御のテスト

TB6605FTG

最近モータドライバを調べたり集めようとしているこの頃です。

TB6605を用いたブラシレスモータシールドを入手したのでデモの動作感を簡単にまとめておきます。

BLDC motor shield は Arduino 互換システム対応のBLDC motor ドライバボードです。9V ~ 24V DCの入力でブラシレスモータを駆動できます。ボード上のポテンショメータで速度を, 4つのスイッチでモード選択を行い制御することができます。

jp.seeedstudio.com

モータドライバはTB6605FTG (TOSHIBA)のものです。

toshiba.semicon-storage.com

今回のデモで利用した部品は3つで,

・Brushless Motor シールド(TB6605FTG)

・Grove - LCD RGB Backlight

・42BLF01 DCブラシレス・モータ

になります。

Grove-LCD RGB Backlight は I2C バスでつなぐことができます。

42BLF01 DCブラシレス・モータ は, DCという言葉が付いているものの, 直流をかけただけでは回転しません。ACモータという類で, 3相の巻き線で, 適切なタイミングで電流を各相のコイルに流します。

www.renesas.com

www.youtube.com

モータ用の BLDC Motor Shield (TB6605) libGitHubから、表示用LCDの Grove-LCD RGB Backlight Library をGithubのリンクからダウンロードします。

Arduino IDEのメニューのスケッチから「ライブラリのインクルード」-「.ZIP形式のライブラリをインストール」を選びます。エクスプローラが開くので、保存した "BLDC_Motor_Shield_TB6605-master.zip" を選択して開くをクリックしてインストールします。Grove の LCDのライブラリも同様にインストールします。

デモプログラム

/*  TB6605_demo.ino


    Copyright (c) 2019 Seeed Technology Co., Ltd.
    Website    : www.seeed.cc
    Create Time: March 2019
    Change Log :

    The MIT License (MIT)

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in
    all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
    THE SOFTWARE.
*/

/*
    User can do following thing by pressing key on board.
    1. Start,stop or brake motor
    2. Change the direction of motor rotation (CW or CCW)

    User can understand current status by reading info display on LCD.
    1. Target speed (tuned by potentiometer)
    2. Current speed detected by HP
    3. Motor status: start, stop or brake
    4. The direction of motor rotation (CW/CCW)
    5. Kp value
*/

#include <rgb_lcd.h>
#include <TimerOne.h>


rgb_lcd lcd;

// digital pins
#define PORT_HP 2                      // digital pin 2 used to check hall pulse from MCD
#define PORT_PWMIN 3                   // digital pin 3 used to output PWM-in signal to MCD as speed control
#define PORT_DIR_CTR 4                 // digital pin 4 used to write CW/CCW switch state from MCD board
#define PORT_DIR 5                     // digital pin 5 used to read CW/CCW switch state from MCD board
#define PORT_START 7                   // digital pin 7 used to read Start switch state from MCD board
#define PORT_BRAKE 8                   // digital pin 8 used to read Brake switch state from MCD board

// analog pins
#define PORT_VR 1                      // analog pin 1 used to connect the potentiometer as target speed input

#define READY 0                        // motor stop state
#define ACTIVE 1                       // motor rotating state

byte mode;                             // motor work state, READY or ACTIVE
volatile long counterVal[2] = {0, 0};  // hall pulse number value to calculate actual rotation speed
byte curCnt = 0;                       // counter to read hall pulse number
bool rotateDir;                        // state of CW/CCW switch
bool rotateStart;                      // state of Start switch
bool rotateBrake;                      // state of Brake switch
int rotateSpeedSetHp;                  // hall pulse number in 150ms against target rotation speed
int rotateSpeedPwm;                    // PWM output to PWM-in pin as a speed command
volatile int hp_freq;                  // hall pulse number counted in 150ms
volatile bool flagReadVR = false;      // flag to read potentiometer value
int oldVRval = 0;                      // potentiometer value read at last time
volatile bool flagReadHP = false;      // flag to read hall pulse number
byte readHPCnt = 0;                    // the timer counter to read hall pulse number

int err1 = 0;                          // error calculated at last time used for PID
int Kp = 4, Ki = 2;                    // pid control parameter, user can modify it for actual used motor

const int hp2Speed =
    100;              // rotation speed(per min) = hp_150ms*(1000ms/150ms)*60s/4 = hp_150ms * 100, paired pole=8/2=4
const int hpMax = 4000 / hp2Speed;     // max rotation speed: 4000RPM, hpMax(150ms): 40
const int vrMax = 245;                 // max VR: 252, 5V -> 1023, 1.16V -> vrMax

void counter();
int get_key(int input);
void timer1_Isr();
void PID_Control(int detectedHp);

/**************************************************************************************
    setup()
    the function is called when a sketch starts. Use it to initialize
    variables, pin modes, start using libraries, etc.
    The function will only run once, after each powerup or reset of the Arduino board.
**************************************************************************************/
void setup() {
    // initialize port for motor control
    pinMode(PORT_START, INPUT);
    pinMode(PORT_BRAKE, INPUT);
    pinMode(PORT_HP, INPUT);
    pinMode(PORT_DIR, INPUT);
    pinMode(PORT_DIR_CTR, OUTPUT);

    // fast PWM for D3(PWMIN) using timer2
    TCCR2B = TCCR2B & 0xF8 | 0x01;        // clock prescale by 1, PWM frequency: 32KHz

    // read potentiometer value to set target rotation speed
    int value = analogRead(PORT_VR);
    rotateSpeedSetHp = map(value, 0, vrMax, 0,
                           hpMax); // convert the value to hall pulse number in 150ms (between 0 and hpMax)
    oldVRval = value;
    int rotateSpeedSet = rotateSpeedSetHp * hp2Speed;
    rotateSpeedPwm = 0;

    // read current state of motor control switches on MCD board
    rotateDir = digitalRead(PORT_DIR);
    rotateStart = digitalRead(PORT_START);
    rotateBrake = digitalRead(PORT_BRAKE);
    mode = READY;

    lcd.begin(16, 2);
    lcd.setRGB(255, 255, 255);

    lcd.setCursor(0, 0);
    lcd.print("T ");
    lcd.print(rotateSpeedSet);
    lcd.setCursor(6, 0);
    lcd.print("RPM");
    lcd.setCursor(6, 1);
    lcd.print("RPM");
    lcd.setCursor(11, 0);
    if (rotateDir == HIGH) {
        lcd.print("CCW");
    } else {
        lcd.print("CW ");
    }

    lcd.setCursor(0, 1);
    lcd.print("C 0   ");
    lcd.setCursor(11, 1);
    if (rotateStart == HIGH) {
        lcd.print("P ");
    } else {
        lcd.print("S ");
    }
    if (rotateBrake == HIGH) {
        lcd.print("  ");
    } else {
        lcd.print("B ");
    }
    lcd.print(Kp);

    Timer1.initialize(25000); // initialize timer1 with 25ms period
    Timer1.attachInterrupt(timer1_Isr);   // enable timer1 interrupt and associate interrupt service routine
    Timer1.start();


    Serial.begin(9600);     // for debug
    Serial.println("== Input Kp:(1-9):");  // Input for Kp
}

/**************************************************************************************
    loop()
    the function loops consecutively,Use it to actively control the Arduino board.
    this loop() constantly checks change of motor control switches, read potentiometer
    value as target rotation speed and get hall pulse frequecncy as actual rotation speed.
    Then achieve target speed with PI control method.
**************************************************************************************/
void loop() {
    static bool flagFirstMotor = false;  // flag of start motor rotating
    bool switchState;

    // check if the state of Brake switch is changed
    switchState = digitalRead(PORT_BRAKE);
    if (switchState != rotateBrake) {
        rotateBrake = switchState;
        if (mode == READY) { // motor is not rotating
            lcd.setCursor(13, 1);
            if (rotateBrake == HIGH) {
                lcd.print(" ");
            } else {
                lcd.print("B");
            }
        } else {
            // mode = ACTIVE, motor is rotating
            if (rotateBrake == LOW) {
                //brake motor
                analogWrite(PORT_PWMIN, 255); // output High to stop output to motor
                detachInterrupt(digitalPinToInterrupt(PORT_HP)); // disable interrupt to counter HP

                lcd.setCursor(2, 1);
                lcd.print("0   ");
                lcd.setCursor(13, 1);
                lcd.print("B");
            } else {
                //restart
                counterVal[0] = 0;
                counterVal[1] = 0;
                curCnt = 0;
                readHPCnt = 0;
                flagReadHP = false;
                rotateSpeedPwm = 0;
                err1 = 0;
                flagFirstMotor = true;
                attachInterrupt(digitalPinToInterrupt(PORT_HP), counter,
                                RISING); // enable interrupt by rising edge of HP signal to count HP

                lcd.setCursor(2, 1);
                lcd.print("0   ");
                lcd.setCursor(13, 1);
                lcd.print(" ");
            }
        }
    }

    // check if the state of Start switch is changed
    switchState = digitalRead(PORT_START);
    if (switchState != rotateStart) {
        rotateStart = switchState;
        if (mode == READY) {
            if (rotateStart == LOW) {
                mode = ACTIVE;
                if (rotateBrake == HIGH) {
                    //start motor
                    counterVal[0] = 0;
                    counterVal[1] = 0;
                    curCnt = 0;
                    readHPCnt = 0;
                    flagReadHP = false;
                    rotateSpeedPwm = 0;
                    err1 = 0;
                    flagFirstMotor = true;
                    attachInterrupt(digitalPinToInterrupt(PORT_HP), counter,
                                    RISING);  // enable interrupt by rising edge of HP signal to count HP
                }

                lcd.setCursor(11, 1);
                lcd.print("S");
            }
        } else {
            //active
            if (switchState == HIGH) {
                //stop motor
                mode = READY;
                analogWrite(PORT_PWMIN, 255);    // output High to stop output to motor
                detachInterrupt(digitalPinToInterrupt(PORT_HP)); // disable interrupt to count HP

                lcd.setCursor(2, 1);
                lcd.print("0   ");
                lcd.setCursor(11, 1);
                lcd.print("P");

                Serial.println("== Input Kp:(1-9):");
            }
        }
    }

    // check if the state of CW/CCW switch is changed
    switchState = digitalRead(PORT_DIR);
    if (switchState != rotateDir) {
        rotateDir = switchState;
        lcd.setCursor(11, 0);
        if (rotateDir == HIGH) {
            lcd.print("CCW");
            for (int i = 200; i < 256; i++) {
                analogWrite(PORT_PWMIN, i);
                delay(10);
            }
            digitalWrite(PORT_DIR_CTR, HIGH);

        } else {
            lcd.print("CW ");
            for (int i = 200; i < 256; i++) {
                analogWrite(PORT_PWMIN, i);
                delay(10);
            }
            digitalWrite(PORT_DIR_CTR, LOW);

        }

    }

    // check if target speed is changed
    if (flagReadVR == true) {
        // read potentiometer value to set target rotation speed
        int value = analogRead(PORT_VR);
        if (value > vrMax) {
            value = vrMax;     // If the speed exceeds 4000, the motor will be damaged
        }

        if ((value > oldVRval + 5) || (value < oldVRval - 5)) {
            rotateSpeedSetHp = map(value, 0, vrMax, 0,
                                   hpMax); // convert the value to hall pulse number in 150ms (between 0 and hpMax)
            int rotateSpeedSet = rotateSpeedSetHp * hp2Speed; // convert to rotation speed for LCD display

            if (rotateSpeedSet < 1000) {
                lcd.setCursor(2, 0);
                lcd.print(rotateSpeedSet);
                lcd.print(" ");
            } else {
                lcd.setCursor(2, 0);
                lcd.print(rotateSpeedSet);
            }



            oldVRval = value;

            Serial.print("target speed: ");
            Serial.println(rotateSpeedSet);
        }
        flagReadVR = false;
    }

    // check current motor rotation speed
    if (mode == ACTIVE && rotateBrake == HIGH) {
        if (flagReadHP == true) {
            flagReadHP = false;
            int speed_val = hp_freq * hp2Speed;

            Serial.print("Hall pulse in 150ms: speed/100  ------- ");
            Serial.println(hp_freq);


            if (flagFirstMotor == true) {  // ignore speed detected at first time which is not valid value.
                flagFirstMotor = false;
            } else {
                if (speed_val < 1000) {
                    lcd.setCursor(2, 1);
                    lcd.print(speed_val);
                    lcd.print(" ");
                } else {
                    lcd.setCursor(2, 1);
                    lcd.print(speed_val);
                }

                PID_Control(hp_freq);     // adjust PWM output to achive target speed based on current speed
            }
        }
    }

    // check if Kp value is changed by input in serial monitor
    if (mode != ACTIVE) {
        if (Serial.available()) {     // check if there is input in serial monitor
            int x = Serial.parseInt(); // look for the next valid integer in the incoming serial stream
            if (x < 10 && x > 0) {
                Kp = x;                   // apply new Kp for PID control
                Serial.print("Kp: ");
                Serial.println(x, DEC);
                lcd.setCursor(15, 1);
                lcd.print(Kp);
            } else {
                Serial.println("Invalid value!");
            }
        }
    }


}

/**************************************************************************************
    counter()
    Interrupt service routine of external interrupt triggered by rising edge of HP signal
**************************************************************************************/
void counter() {
    counterVal[curCnt & 0x01] += 1;
}

/**************************************************************************************
    timer1_Isr()
    Timer1 interrupt servie routine
    Read potentiometer value and check hall pulse number at specified timing
**************************************************************************************/
void timer1_Isr() {
    static byte readVRCnt = 0;   // the timer counter to read potentiometer

    if (readVRCnt == 4) {       // period: 25ms*5 = 125ms
        flagReadVR = true;        // set flag to tell loop() to read potentiometer value
        readVRCnt = 0;
    } else {
        readVRCnt++;
    }

    if (mode == ACTIVE && rotateBrake == HIGH) {
        if (readHPCnt == 5) {      // period: 25ms*6 = 150ms
            flagReadHP = true;       // set flag to tell loop() to read HP
            curCnt++;
            readHPCnt = 0;

            if ((curCnt & 0x01) == 0) {
                hp_freq = counterVal[1];
                counterVal[1] = 0;     // clear for new data
            } else {
                hp_freq = counterVal[0];
                counterVal[0] = 0;     // clear for new data
            }
        } else {
            readHPCnt++;
        }
    }

}

/**************************************************************************************
    PID_Control()
    Incremental PID control based on current speed detected by Hall sensor
    int detectedHp: hall pulse number in 150ms
**************************************************************************************/
const int pidOver1_U = 4;  // upper limit value
const int pidOver1_L = -4; // lower limit value
const int pidOver2_U = 2;  // upper limit value in case error is small
const int pidOver2_L = -2; // lower limit value in case error is small
int Kp_s = 4;  // Kp value during starting motor, user can modify it for actual used motor
void PID_Control(int detectedHp) {
    int err;
    int bias;
    int pidOver_U;
    int pidOver_L;

    err = rotateSpeedSetHp - detectedHp;     // error = set value - actual value

    Serial.print("hp e: ");
    Serial.println(err);

    if (detectedHp == 0) {
        bias = Kp_s * (err - err1);            // do only when starting motor rotation and speed is zero
    } else {
        bias =  Kp * (err - err1) + Ki * err;  // do always when motor is rotating and speed is not zero
        Serial.print("bias: ");
        Serial.println(bias);

        if (err < 2 && err > -2) {
            pidOver_U = pidOver2_U;
            pidOver_L = pidOver2_L;
        } else {
            pidOver_U = pidOver1_U;
            pidOver_L = pidOver1_L;
        }

        if (bias > pidOver_U) {
            bias = pidOver_U;
        } else if (bias < pidOver_L) {
            bias = pidOver_L;
        }
    }

    Serial.print("set bias: ");
    Serial.println(bias);

    rotateSpeedPwm += bias;
    err1 = err;

    Serial.print("set PWM value: ");
    Serial.println(rotateSpeedPwm, DEC);

    if (rotateSpeedPwm > 255) {          // PWM value between 0 and 255
        rotateSpeedPwm = 255;
    }
    if (rotateSpeedPwm < 1) {
        rotateSpeedPwm = 1;
    }

    int pwmIn = 0xFF - rotateSpeedPwm;   // low active
    Serial.println(pwmIn, DEC);
    analogWrite(PORT_PWMIN, pwmIn);      // change PWM duty to change rotation speed
}

動作風景

24V の電源を Motor Shield に投入し, プログラムを書き込んで, ターミナル上でゲインの設定を行い, shield 上の Start スイッチでモータの駆動を始めます。動作風景を以下の動画で示します。

www.youtube.com

運動学

運動学について

運動学とは, ロボットの「関節の変位や長さ」と「先端位置と姿勢」の関係式を数式で表したものです。

運動学には順運動学と逆運動学があり,

・順運動学

ロボットアームに各関節と角度を与えて, その先端位置と姿勢を求める問題を扱います。例えば, 関節空間から座標空間へ変換する行列を求めるため, ロボットアームのリンク L_i と関節 J_i の関係を D-H 法 (Denavit-Hartenberg 法) で表します。

・逆運動学

逆運動学とは, ロボットアームの制御したい点の位置と姿勢を与えて, 各関節の角度または変位を求める問題を扱います。

D-H 法

各関節に対する座標系を以下のように設定します

① ベースに基準座標系を設定する。回転関節の場合, z_B 軸を回転軸に, 直動関節の場合 z_B 軸を直動変位の方向に設定します。また残りの座標系を右手系により決定します。

② 次に各リンクごとに座標系を設定するとき, 関節  J_i の回転軸または直動変位の方向に  z_i を設定します。 z_i z_{i-1} 軸の共通法線方向に x_i を, 右手系により y_i 軸を設定します。

myCobot ことはじめ

myCobot を入手しました

myCobot が界隈で流行っているようで, それに乗っかり購入しました。

www.elephantrobotics.com

Elephant robotics 社は中国の 深セン市 南山区にあるロボット工学について製造・設計を行うテクノロジー会社のようです。

"Elephant Robotics is a technology firm specializing in the design and production of robotics, development and applications of operating system and intelligent manufacturing services in industry, commerce, education, scientific research, home and etc."

昨年の12月末ほどに注文しました。送料を含めると65000円ほどでした。注文して1週間ほどで到着しました。

f:id:Manao55:20210123193128j:plain
届いたもの

f:id:Manao55:20210123193650j:plain
開封

f:id:Manao55:20210123193910j:plain

折りたたまれて収納されています。付属品は, ジャンパワイヤ, ACアダプタ (8.33V) が内包されています。

書き込み済みのソフトにはティーチングプレイバックができるものが書き込まれています。

kotobank.jp

f:id:Manao55:20210124142701j:plain
起動時画面

モーションをRAMに保存するか, flushに保存するかの選択ができます。

f:id:Manao55:20210124142949j:plain
Recode mode ( RAM保存, flush保存 が可能)

モーションを保存させていきます。

f:id:Manao55:20210124143346j:plain
モーション保存中の画面
f:id:Manao55:20210124143435j:plain
プレイバック中の画面

では動かしてみます。

www.youtube.com

RAM mode で動かしていますが, 滑らかです。

いろいろ遊んでみようと思います。

連立方程式を解く

ガウスジョルダン

ja.wikipedia.org

{ \displaystyle
a_{11}x_{1} + a_{12}x_{2} + a_{13}x_{3} = b_{1} ... ①
}
{ \displaystyle
a_{21}x_{1} + a_{22}x_{2} + a_{23}x_{3} = b_{2} ... ②
}
{ \displaystyle
a_{31}x_{1} + a_{32}x_{2} + a_{33}x_{3} = b_{3} ... ③
}

①式を a_{11} で割り, x_1 の係数を 1 にして, ①^{'} 式を作ります。

②式から ①^{'} 式を a_{21} 倍したものを引き, ②^{'} 式を作ります。

③式から ①^{'} 式を a_{31} 倍したものを引き, ③^{'} 式を作ります。

{ \displaystyle
x_{1} + a_{12}^{'}x_{2} + a_{13}^{'}x_{3} = b_{1}^{'} ... ①^{'}
}
{ \displaystyle
a_{22}^{'}x_{2} + a_{23}^{'}x_{3} = b_{2}^{'} ... ②^{'}
}
{ \displaystyle
a_{32}^{'}x_{2} + a_{33}^{'}x_{3} = b_{3}^{'} ... ③^{'}
}

②^{'} 式を a_{22}^{'} で割り, x_2 の係数を 1 にして, ①^{''} 式を作ります。

①^{'} 式から ②^{'} 式を a_{12}^{'} 倍したものを引き, ②^{''} 式を作ります。

③^{'} 式から ②^{'} 式を a_{32}^{'} 倍したものを引き, ③^{''} 式を作ります。

{ \displaystyle
x_{1}       + a_{13}^{''}x_{3} = b_{1}^{''} ... ①^{''}
}
{ \displaystyle
x_{2} + a_{23}^{''}x_{3} = b_{2}^{''} ... ②^{''}
}
{ \displaystyle
a_{33}^{''}x_{3} = b_{3}^{''} ... ③^{''}
}

③^{''} 式を a_{33}^{''} で割ります。

①^{''} 式を ③^{'''} × a_{13}^{''} で引きます。

②^{''} 式を ③^{'''} × a_{23}^{''} で引きます。

{ \displaystyle
x_{1} = b_{1}^{'''} ... ①^{'''}
}
{ \displaystyle
x_{2} = b_{2}^{'''} ... ②^{'''}
}
{ \displaystyle
x_{3} = b_{3}^{'''} ... ③^{'''}
}

プログラム

#include <iostream>
#include <iomanip>
using namespace std;

#define N 3  /* 元 */

int main(){
    static double a[N][N+1] = {  /* 係数行列 */
        {1.0  ,2.0  ,3.0  ,2.0 },
        {2.0  ,2.0  ,3.0 ,1.0},
        {2.0 ,2.0  ,1.0  ,-1.0 }
    };
    double p,d;
    int i,j,k;

    for (k=0;k<N;k++){
        p=a[k][k];                   /* ピボット係数 */
        for (j=k;j<N+1;j++)          /* ピボット行をpで除算 */
            a[k][j]=a[k][j]/p;
        for (i=0;i<N;i++){           /* ピボット列の掃き出し */
            if (i!=k){
                d = a[i][k];
                for (j=k;j<N+1;j++)
                    a[i][j] = a[i][j] -d*a[k][j];
            }
        }
    }

    for(k=0;k<N;k++)
        cout << "x" << k+1 << "=" << a[k][N] << endl;

    return 0;
}

実行結果

x1=-1
x2=0
x3=1