ヒロキ モリ / Mbed 2 deprecated MotorPIcontrol

インクリメンタルエンコーダを使ったDCモータの速度制御プログラム

Dependencies:   mbed QEI mbed-rtos motordriver SDFileSystem Encoder

main.cpp@0:ada8c03739c0, 2019-03-22 (annotated)

Committer:
porizou3
Date:
Fri Mar 22 10:20:53 2019 +0000
Revision:
0:ada8c03739c0
commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
porizou3 0:ada8c03739c0 1 #include "mbed.h"
porizou3 0:ada8c03739c0 2 #include "rtos.h"
porizou3 0:ada8c03739c0 3
porizou3 0:ada8c03739c0 4 #include "Encoder.h"
porizou3 0:ada8c03739c0 5 #include "SDFileSystem.h"
porizou3 0:ada8c03739c0 6 #include "motordriver.h"
porizou3 0:ada8c03739c0 7
porizou3 0:ada8c03739c0 8 #define V_CONTROL_CYCLE 0.01 //速度制御周期[s]
porizou3 0:ada8c03739c0 9
porizou3 0:ada8c03739c0 10 #define Kp 0.03 //比例ゲイン
porizou3 0:ada8c03739c0 11 #define Ki 0.578 //積分ゲイン
porizou3 0:ada8c03739c0 12
porizou3 0:ada8c03739c0 13 #define Vmax 8.2 //モータの電源電圧[V]
porizou3 0:ada8c03739c0 14
porizou3 0:ada8c03739c0 15 DigitalOut led(LED1);
porizou3 0:ada8c03739c0 16
porizou3 0:ada8c03739c0 17 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
porizou3 0:ada8c03739c0 18
porizou3 0:ada8c03739c0 19 Serial pc(USBTX, USBRX); // tx, rx
porizou3 0:ada8c03739c0 20
porizou3 0:ada8c03739c0 21 Encoder Enc(p21, p22, 12, 0.001, 0.01);
porizou3 0:ada8c03739c0 22 Motor motor(p25 , p23 , p24 , 10000);
porizou3 0:ada8c03739c0 23
porizou3 0:ada8c03739c0 24 float accError = 0.0; //偏差の積分値
porizou3 0:ada8c03739c0 25 float prevError = 0.0; //前回の偏差
porizou3 0:ada8c03739c0 26
porizou3 0:ada8c03739c0 27 float Vref = 0.0; //速度の目標値[rad/s]
porizou3 0:ada8c03739c0 28 float V = 0.0;
porizou3 0:ada8c03739c0 29
porizou3 0:ada8c03739c0 30 double T = 0.0;
porizou3 0:ada8c03739c0 31
porizou3 0:ada8c03739c0 32
porizou3 0:ada8c03739c0 33 /* 速度制御スレッド */
porizou3 0:ada8c03739c0 34 void VelocityThread(void const *argument) {
porizou3 0:ada8c03739c0 35
porizou3 0:ada8c03739c0 36 V = -Enc.getVelocity(); //速度の値を取得[rad/s]
porizou3 0:ada8c03739c0 37
porizou3 0:ada8c03739c0 38 float Error = Vref - V; //偏差の計算
porizou3 0:ada8c03739c0 39
porizou3 0:ada8c03739c0 40 accError += (Error + prevError) / 2.0 * V_CONTROL_CYCLE; //偏差の積分値の計算
porizou3 0:ada8c03739c0 41
porizou3 0:ada8c03739c0 42 float Vout = Kp * Error + Ki * accError; //出力電圧を計算
porizou3 0:ada8c03739c0 43
porizou3 0:ada8c03739c0 44 motor.rotate(Vout / Vmax); //モータドライバに出力
porizou3 0:ada8c03739c0 45
porizou3 0:ada8c03739c0 46 prevError = Error;
porizou3 0:ada8c03739c0 47
porizou3 0:ada8c03739c0 48 T += V_CONTROL_CYCLE;
porizou3 0:ada8c03739c0 49 }
porizou3 0:ada8c03739c0 50
porizou3 0:ada8c03739c0 51 int main() {
porizou3 0:ada8c03739c0 52
porizou3 0:ada8c03739c0 53 //pc.baud(115200);
porizou3 0:ada8c03739c0 54
porizou3 0:ada8c03739c0 55 RtosTimer Velocity(VelocityThread); //速度制御
porizou3 0:ada8c03739c0 56
porizou3 0:ada8c03739c0 57 Velocity.start(V_CONTROL_CYCLE * 1000);
porizou3 0:ada8c03739c0 58
porizou3 0:ada8c03739c0 59 while(1) {
porizou3 0:ada8c03739c0 60 Vref = 700;
porizou3 0:ada8c03739c0 61
porizou3 0:ada8c03739c0 62 pc.printf("%f,%f\n\r",Vref, V);
porizou3 0:ada8c03739c0 63 led = !led;
porizou3 0:ada8c03739c0 64 wait(0.001);
porizou3 0:ada8c03739c0 65 }
porizou3 0:ada8c03739c0 66 }