Created
March 25, 2020 09:14
-
-
Save sbasami/933ef56bfece2b731cc31651da47cabc to your computer and use it in GitHub Desktop.
Teensy LCでIPD制御を行うプログラム
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
//PWM出力に関しての詳細はhttps://www.pjrc.com/teensy/td_pulse.html | |
//エンコーダに関しての詳細はhttps://www.pjrc.com/teensy/td_libs_Encoder.html | |
//タイマー割込みに関しての詳細はhttps://www.pjrc.com/teensy/td_timing_IntervalTimer.html | |
//Teensyの標準ヘッダファイル | |
#include <Encoder.h> | |
//定数の宣言 | |
#define GEAR (32*33*35*38) / (15*14*13*10)//モータのギア比 | |
#define PULSE 12.0 //エンコーダのパルス数 | |
#define Ts 0.001 //サンプリング周期[s] | |
#define PWM 9 //PWM出力ピン | |
#define DIR 10 //モータ回転方向指定ピン | |
#define volt 5 //モータの電源電圧[V] | |
//グローバル変数の宣言 | |
float r = 0;//目標値[rad] | |
float val; //シリアル通信で送られてくる目標値[deg] | |
//クラスの宣言 | |
IntervalTimer myTimer; //タイマー割り込み | |
Encoder enc(11, 12); //エンコーダ | |
void setup() { | |
Serial.begin(115200); | |
pinMode(PWM,OUTPUT); | |
pinMode(DIR,OUTPUT); | |
analogWriteFrequency(PWM, 46875);//モータドライバへ出力するPWMの周波数を変更 | |
analogWriteResolution(10);//PWMの分解能を変更 | |
myTimer.priority(200);//タイマーの優先度を128から200に変更 | |
myTimer.begin(interrupt, Ts*1000000);//タイマー割り込み開始 | |
} | |
void interrupt() { | |
float Kp = 40.22, Ki = 672.5, Kd = 0.6012;//制御ゲイン | |
static float y_old = 0;//過去の出力 | |
static float integral = 0;//積分 | |
float y;//出力[rad] | |
float u;//入力[V] | |
float e;//偏差 | |
float duty;//PWMの出力 | |
float cnt;//エンコーダのカウント | |
cnt = enc.read();//エンコーダの値を取得 | |
y = (cnt * 2 * PI) / (PULSE * GEAR);//現在角度[rad] | |
e = r - y;//偏差[rad] | |
integral += e * Ts;//積分の計算 | |
u = -Kp*y + Ki*integral - Kd*(y-y_old)/Ts;//I-PD制御 | |
y_old = y;//出力を格納 | |
//入力制限とアンチワインドアップ制御 | |
if (u > volt) {u= volt; integral-=e*Ts;} | |
else if (u < -volt) {u=-volt; integral-=e*Ts;} | |
duty = u * 1023 / volt;//入力電圧をDutyに変換 | |
//モータへの出力 | |
if (duty >= 0) | |
{ | |
digitalWrite(DIR, HIGH); | |
analogWrite(PWM, (int)duty); | |
} | |
else { | |
digitalWrite(DIR, LOW); | |
analogWrite(PWM, (int)-duty); | |
} | |
//ログデータの出力 | |
Serial.print(val,4); | |
Serial.print(" : "); | |
Serial.print(u,4); | |
Serial.print(" : "); | |
Serial.println(y*360/(2*PI),4); | |
} | |
void loop() { | |
//目標値の取得 | |
if(KICK_SERIAL()) | |
{ | |
val = SERIAL_VAL();//シリアル通信で送られてきた目標値[deg] | |
r = val*2*PI/360;//[deg]を[rad]に変換して目標値に設定 | |
} | |
delay(10); | |
} | |
bool KICK_SERIAL() { | |
bool flag = false; | |
if (Serial.available() > 0) | |
{ | |
flag = true; | |
//delay(20); | |
} | |
return flag; | |
} | |
int SERIAL_VAL() { | |
byte data_size = Serial.available(); | |
byte buf[data_size], degree = 1; | |
long recv_data = 0, dub = 1; | |
bool minus = 0; | |
for (byte i = 0 ; i < data_size ; i++) | |
{ | |
buf[i] = Serial.read(); | |
if (buf[i] >= '0' && buf[i] <= '9') buf[i] -= '0'; | |
else { | |
if (buf[0] == '-') minus = 1; | |
else degree = 0; | |
} | |
} | |
if (degree == 1) degree = data_size - minus; | |
for (byte i = 0 ; i < degree ; i++) | |
{ | |
recv_data += buf[(data_size - 1) - i] * dub; | |
dub *= 10; | |
} | |
if (minus) recv_data *= -1; | |
return recv_data; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment