ヒロキ モリ / Mbed 2 deprecated Motor_P_PositionControl

DC Motor PositionControl

Dependencies:   mbed QEI mbed-rtos motordriver Encoder

main.cpp@0:bd1837f054f4, 2019-06-09 (annotated)

Committer:
porizou3
Date:
Sun Jun 09 09:27:44 2019 +0000
Revision:
0:bd1837f054f4
firstcommit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
porizou3 0:bd1837f054f4 1 #include "mbed.h"
porizou3 0:bd1837f054f4 2 #include "rtos.h"
porizou3 0:bd1837f054f4 3
porizou3 0:bd1837f054f4 4 #include "Encoder.h"
porizou3 0:bd1837f054f4 5
porizou3 0:bd1837f054f4 6 #include "motordriver.h"
porizou3 0:bd1837f054f4 7
porizou3 0:bd1837f054f4 8 #define V_CONTROL_CYCLE 0.01 //速度制御周期[s]
porizou3 0:bd1837f054f4 9 #define P_CONTROL_CYCLE 0.1 //位置制御周期[s]
porizou3 0:bd1837f054f4 10
porizou3 0:bd1837f054f4 11 #define Kp 0.03 //速度比例ゲイン
porizou3 0:bd1837f054f4 12 #define Ki 0.578 //速度積分ゲイン
porizou3 0:bd1837f054f4 13
porizou3 0:bd1837f054f4 14 #define Kpp 2.0 //位置比例ゲイン
porizou3 0:bd1837f054f4 15
porizou3 0:bd1837f054f4 16 #define Vmax 8.2 //モータの電源電圧[V]
porizou3 0:bd1837f054f4 17 #define OMEGA_MAX 400//モータの最大回転速度[rad/s]
porizou3 0:bd1837f054f4 18
porizou3 0:bd1837f054f4 19 DigitalOut led(LED1);
porizou3 0:bd1837f054f4 20
porizou3 0:bd1837f054f4 21
porizou3 0:bd1837f054f4 22
porizou3 0:bd1837f054f4 23 Serial pc(USBTX, USBRX); // tx, rx
porizou3 0:bd1837f054f4 24
porizou3 0:bd1837f054f4 25 Encoder Enc(p21, p22, 60, 0.001, 0.01);
porizou3 0:bd1837f054f4 26 Motor motor(p25 , p23 , p24 , 10000);
porizou3 0:bd1837f054f4 27
porizou3 0:bd1837f054f4 28 float accError = 0.0; //偏差の積分値
porizou3 0:bd1837f054f4 29 float prevError = 0.0; //前回の偏差
porizou3 0:bd1837f054f4 30
porizou3 0:bd1837f054f4 31 float Pref = 0.0; //位置の目標値[rad]
porizou3 0:bd1837f054f4 32
porizou3 0:bd1837f054f4 33 float Vref = 0.0; //速度の目標値[rad/s]
porizou3 0:bd1837f054f4 34
porizou3 0:bd1837f054f4 35 /* 位置制御スレッド */
porizou3 0:bd1837f054f4 36 void PositionThread(void const *argument) {
porizou3 0:bd1837f054f4 37 float P = Enc.getAngle();//角度の値を取得[rad/s]
porizou3 0:bd1837f054f4 38
porizou3 0:bd1837f054f4 39 float Error = Pref - P;//偏差の計算
porizou3 0:bd1837f054f4 40
porizou3 0:bd1837f054f4 41
porizou3 0:bd1837f054f4 42 Vref = Error * Kpp; //出力速度の計算
porizou3 0:bd1837f054f4 43 if(Vref > OMEGA_MAX) Vref = OMEGA_MAX;
porizou3 0:bd1837f054f4 44 if(Vref < -OMEGA_MAX) Vref = -OMEGA_MAX;
porizou3 0:bd1837f054f4 45 }
porizou3 0:bd1837f054f4 46
porizou3 0:bd1837f054f4 47
porizou3 0:bd1837f054f4 48 /* 速度制御スレッド */
porizou3 0:bd1837f054f4 49 void VelocityThread(void const *argument) {
porizou3 0:bd1837f054f4 50
porizou3 0:bd1837f054f4 51 float V = Enc.getVelocity(); //速度の値を取得[rad/s]
porizou3 0:bd1837f054f4 52
porizou3 0:bd1837f054f4 53 float Error = Vref - V; //偏差の計算
porizou3 0:bd1837f054f4 54
porizou3 0:bd1837f054f4 55 accError += (Error + prevError) / 2.0 * V_CONTROL_CYCLE; //偏差の積分値の計算
porizou3 0:bd1837f054f4 56
porizou3 0:bd1837f054f4 57 float Vout = Kp * Error + Ki * accError; //出力電圧を計算
porizou3 0:bd1837f054f4 58
porizou3 0:bd1837f054f4 59 motor.rotate(Vout / Vmax); //モータドライバに出力
porizou3 0:bd1837f054f4 60
porizou3 0:bd1837f054f4 61 prevError = Error; //偏差の更新
porizou3 0:bd1837f054f4 62 }
porizou3 0:bd1837f054f4 63
porizou3 0:bd1837f054f4 64 int main() {
porizou3 0:bd1837f054f4 65
porizou3 0:bd1837f054f4 66 //pc.baud(115200);
porizou3 0:bd1837f054f4 67
porizou3 0:bd1837f054f4 68 RtosTimer Velocity(VelocityThread); //速度制御
porizou3 0:bd1837f054f4 69 RtosTimer Position(PositionThread); //位置制御
porizou3 0:bd1837f054f4 70
porizou3 0:bd1837f054f4 71 Velocity.start(V_CONTROL_CYCLE * 1000);
porizou3 0:bd1837f054f4 72 Position.start(P_CONTROL_CYCLE * 1000);
porizou3 0:bd1837f054f4 73 //motor.rotate(1.0);
porizou3 0:bd1837f054f4 74
porizou3 0:bd1837f054f4 75 while(1) {
porizou3 0:bd1837f054f4 76 Pref = 2*PI / 4 * 42;
porizou3 0:bd1837f054f4 77 wait(1.0);
porizou3 0:bd1837f054f4 78 Pref = 0.0;
porizou3 0:bd1837f054f4 79 wait(1.0);
porizou3 0:bd1837f054f4 80 }
porizou3 0:bd1837f054f4 81 }