デジタル電流制御
デジタル制御
デジタル制御についての備忘録としてここに記します。
制御プラント
以下の単純な単相ハーフブリッジインバータを制御プラントにして, デジタル制御の議論を行います。
デジタル制御を実現するにはマイクロコントローラを使用します.
Microcontrollers (MCU)
デジタル制御を実現するための組み込みシステムについて
・CPU, RAM, ROM
・A/D, PWM, Timer
を必要とします。
デジタル電流制御
以下の3点に注意します。
・A/Dコンバータを用いた電流検出
サンプリング/変換の時間遅れ
量子化ビットエラー
・CPU内での計算
実行時間の遅延
キャリア信号/基準値の分解能
これらの要件をクリアするMCUが制御に必要になります。
ダブルアップデートでのタイミングチャート
以下にPWMキャリアの山と谷をサンプリングしたダブルアップデートの模式図を示します。
PWMキャリアの山の部分と谷の部分で電流のサンプリングを行い, 各サンプリング間にAD変換と演算を行い, サンプリングして次のサンプリング点にてPWM制御演算を更新することによって制御を実現します。更新タイミングの遅れ時間は, = = となります。
また山と谷のサンプリングではなく, 山のみのサンプリングを行った際の遅れ時間は, = = となります。
デジタル電流制御
連続時間領域における制御ブロック図を示します。
・ : 伝達関数内の電流フィードバックゲイン
・ : 遅れ時間
・ : ゼロ次ホールド関数
次にデジタル時間領域におけるパルス伝達関数のブロック図を示します。
・ : パルス伝達関数内の電流フィードバックゲイン
・ : サンプリング遅れ時間
パルス伝達関数
・クローズドループパルス伝達関数
・極 z を求めると,
となります。
z 平面での安定性
s平面下では極 は収束, 発散を. は振動性を表しています。
は負に配置されれば収束し, が0であれば振動はなくなります。
z平面下では, は収束性を, は振動性を示します。
が単位円内にあれば収束し, の角度が0であれば振動はなくなります。
パルス伝達関数
ここで, 極の議論に戻ります。
・安定限界ゲイン
このとき,
となり, 安定限界に極配置されます。
・クリティカルゲイン
のとき,
となり, 臨界制動の挙動をとります。
時間領域における応答
では時間領域における応答を見てみましょう. 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()
のとき, クリティカルゲインとなり振動なく収束します。
のとき, クリティカルゲイン時より早く収束しますが振動成分が残ります。
のとき, 収束はさらに早くなるもののかなり振動の大きい応答になります。
デッドビート制御
・遅延要素
MCU内のサンプリング遅延
電流応答の遅れ(インダクタンスの要素)
少なくとも 2サンプリングは電流応答の遅れがある
デッドビート制御の伝達関数
・2サンプリング遅延のクローズドループ伝達関数であると仮定します
・クローズドループ伝達関数
・ から得られる伝達関数の結果
・極
・実装
・時間領域表現
ブロック図
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()
2サンプリングで収束することがわかります。
デジタル電流制御②
前回に続いて, 今回は電流制御の応用についての議論を行います.
デッドビート制御
・遅延要素
MCU内のサンプリング遅延
電流応答の遅れ(インダクタンスの要素)
少なくとも 2サンプリングは電流応答の遅れがある
デッドビート制御の伝達関数
・2サンプリング遅延のクローズドループ伝達関数であると仮定します
・クローズドループ伝達関数
・ から得られる伝達関数の結果
・極
・実装
・時間領域表現
ブロック図
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()
2サンプリングで収束することがわかります。
デジタル電流制御
デジタル制御①
デジタル制御についての備忘録としてここに記します。
制御プラント
以下の単純な単相ハーフブリッジインバータを制御プラントにして, デジタル制御の議論を行います。
デジタル制御を実現するにはマイクロコントローラを使用します.
Microcontrollers (MCU)
デジタル制御を実現するための組み込みシステムについて
・CPU, RAM, ROM
・A/D, PWM, Timer
を必要とします。
デジタル電流制御
以下の3点に注意します。
・A/Dコンバータを用いた電流検出
サンプリング/変換の時間遅れ
量子化ビットエラー
・CPU内での計算
実行時間の遅延
キャリア信号/基準値の分解能
これらの要件をクリアするMCUが制御に必要になります。
ダブルアップデートでのタイミングチャート
以下にPWMキャリアの山と谷をサンプリングしたダブルアップデートの模式図を示します。
PWMキャリアの山の部分と谷の部分で電流のサンプリングを行い, 各サンプリング間にAD変換と演算を行い, サンプリングして次のサンプリング点にてPWM制御演算を更新することによって制御を実現します。更新タイミングの遅れ時間は, = = となります。
また山と谷のサンプリングではなく, 山のみのサンプリングを行った際の遅れ時間は, = = となります。
デジタル電流制御
連続時間領域における制御ブロック図を示します。
・ : 伝達関数内の電流フィードバックゲイン
・ : 遅れ時間
・ : ゼロ次ホールド関数
次にデジタル時間領域におけるパルス伝達関数のブロック図を示します。
・ : パルス伝達関数内の電流フィードバックゲイン
・ : サンプリング遅れ時間
パルス伝達関数
・クローズドループパルス伝達関数
・極 z を求めると,
となります。
z 平面での安定性
s平面下では極 は収束, 発散を. は振動性を表しています。
は負に配置されれば収束し, が0であれば振動はなくなります。
z平面下では, は収束性を, は振動性を示します。
が単位円内にあれば収束し, の角度が0であれば振動はなくなります。
パルス伝達関数
ここで, 極の議論に戻ります。
・安定限界ゲイン
このとき,
となり, 安定限界に極配置されます。
・クリティカルゲイン
のとき,
となり, 臨界制動の挙動をとります。
時間領域における応答
では時間領域における応答を見てみましょう. 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()
のとき, クリティカルゲインとなり振動なく収束します。
のとき, クリティカルゲイン時より早く収束しますが振動成分が残ります。
のとき, 収束はさらに早くなるもののかなり振動の大きい応答になります。
TB6605FTG BLDC motor shield によるBLDC Motor 速度制御のテスト
TB6605FTG
最近モータドライバを調べたり集めようとしているこの頃です。
TB6605を用いたブラシレスモータシールドを入手したのでデモの動作感を簡単にまとめておきます。
BLDC motor shield は Arduino 互換システム対応のBLDC motor ドライバボードです。9V ~ 24V DCの入力でブラシレスモータを駆動できます。ボード上のポテンショメータで速度を, 4つのスイッチでモード選択を行い制御することができます。
モータドライバはTB6605FTG (TOSHIBA)のものです。
今回のデモで利用した部品は3つで,
・Brushless Motor シールド(TB6605FTG)
・Grove - LCD RGB Backlight
・42BLF01 DCブラシレス・モータ
になります。
Grove-LCD RGB Backlight は I2C バスでつなぐことができます。
42BLF01 DCブラシレス・モータ は, DCという言葉が付いているものの, 直流をかけただけでは回転しません。ACモータという類で, 3相の巻き線で, 適切なタイミングで電流を各相のコイルに流します。
モータ用の BLDC Motor Shield (TB6605) libをGitHubから、表示用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 スイッチでモータの駆動を始めます。動作風景を以下の動画で示します。
運動学
運動学について
運動学とは, ロボットの「関節の変位や長さ」と「先端位置と姿勢」の関係式を数式で表したものです。
運動学には順運動学と逆運動学があり,
・順運動学
ロボットアームに各関節と角度を与えて, その先端位置と姿勢を求める問題を扱います。例えば, 関節空間から座標空間へ変換する行列を求めるため, ロボットアームのリンク と関節 の関係を D-H 法 (Denavit-Hartenberg 法) で表します。
・逆運動学
逆運動学とは, ロボットアームの制御したい点の位置と姿勢を与えて, 各関節の角度または変位を求める問題を扱います。
D-H 法
各関節に対する座標系を以下のように設定します
① ベースに基準座標系を設定する。回転関節の場合, 軸を回転軸に, 直動関節の場合 軸を直動変位の方向に設定します。また残りの座標系を右手系により決定します。
② 次に各リンクごとに座標系を設定するとき, 関節 の回転軸または直動変位の方向に を設定します。 と 軸の共通法線方向に を, 右手系により 軸を設定します。
myCobot ことはじめ
myCobot を入手しました
myCobot が界隈で流行っているようで, それに乗っかり購入しました。
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週間ほどで到着しました。
折りたたまれて収納されています。付属品は, ジャンパワイヤ, ACアダプタ (8.33V) が内包されています。
書き込み済みのソフトにはティーチングプレイバックができるものが書き込まれています。
モーションをRAMに保存するか, flushに保存するかの選択ができます。
モーションを保存させていきます。
では動かしてみます。
RAM mode で動かしていますが, 滑らかです。
いろいろ遊んでみようと思います。
連立方程式を解く
ガウス・ジョルダン法
①式を で割り, の係数を 1 にして, 式を作ります。
②式から 式を 倍したものを引き, 式を作ります。
③式から 式を 倍したものを引き, 式を作ります。
式を で割り, の係数を 1 にして, 式を作ります。
式から 式を 倍したものを引き, 式を作ります。
式から 式を 倍したものを引き, 式を作ります。
式を で割ります。
式を × で引きます。
式を × で引きます。
プログラム
#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