DC motor control program using TA7291P type H bridge driver and rotary encoder with A, B phase.
Dependencies: QEI mbed-rtos mbed
Fork of DCmotor by
controller.h@14:02411880ffb9, 2013-03-12 (annotated)
- Committer:
- kosaka
- Date:
- Tue Mar 12 04:32:22 2013 +0000
- Revision:
- 14:02411880ffb9
- Parent:
- 13:ba71733c11d7
130303;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kosaka | 12:459af534d1ee | 1 | #ifndef __controller_h |
kosaka | 12:459af534d1ee | 2 | #define __controller_h |
kosaka | 12:459af534d1ee | 3 | |
kosaka | 13:ba71733c11d7 | 4 | #define PI 3.14159265358979 // 円周率πの定義 |
kosaka | 13:ba71733c11d7 | 5 | |
kosaka | 13:ba71733c11d7 | 6 | /*********** 使用するポート、サンプル時間、制御ゲインなどの設定 (ここから) ***************/ |
kosaka | 13:ba71733c11d7 | 7 | //#define SIMULATION // ブラシ付DCモータの特性をシミュレーションするとき、コメント外す |
kosaka | 13:ba71733c11d7 | 8 | //#define USE_CURRENT_CONTROL // 電流制御ありのとき、コメント外す Current control on. Comment if current control off. |
kosaka | 13:ba71733c11d7 | 9 | |
kosaka | 13:ba71733c11d7 | 10 | //#define DEADZONE_PLUS 0//1. // deadzone of plus side |
kosaka | 13:ba71733c11d7 | 11 | //#define DEADZONE_MINUS 0//-1.5 // deadzone of minus side |
kosaka | 12:459af534d1ee | 12 | |
kosaka | 13:ba71733c11d7 | 13 | // エンコーダの設定 |
kosaka | 13:ba71733c11d7 | 14 | #define N_ENC (24*4) // エンコーダ分解能(4逓倍)=1回転分のパルス数×4 "*4": QEI::X4_ENCODING. Number of pulses in one revolution(=360 deg) of rotary encoder. |
kosaka | 13:ba71733c11d7 | 15 | #define CH_A p29 // A相用ポート A phase port |
kosaka | 13:ba71733c11d7 | 16 | #define CH_B p30 // B相用ポート A phase port |
kosaka | 12:459af534d1ee | 17 | |
kosaka | 13:ba71733c11d7 | 18 | // タイマーのサンプル周期等 |
kosaka | 14:02411880ffb9 | 19 | //#define PWM_FREQ 10000.0 // [Hz], PWMチョッピング周波数 pwm freq. (> 1/(DEAD_TIME*10)) |
kosaka | 13:ba71733c11d7 | 20 | //#define DEADTIME 0.0001 // [s], デッドタイム(モータに加える電圧の正負が変わるときに上下アーム短絡を避けるために同時オフする時間) // [s], deadtime to be set between plus volt. to/from minus |
kosaka | 13:ba71733c11d7 | 21 | #define TS0 0.002 // [s], timerTS0のサンプル時間(電流制御用) sampling time (priority highest: Ticker IRQ) of motor current i control PID using timer interrupt |
kosaka | 13:ba71733c11d7 | 22 | #define TS1 0.002 // [s], timerTS1のサンプル時間(位置制御用) sampling time (priority high: RtosTimer) of motor angle th PID using rtos-timer |
kosaka | 13:ba71733c11d7 | 23 | #define TS2 0.2 // [s], timerTS2のサンプル時間(不使用) sampling time (priority =main(): precision 4ms) to save data to PC using thread. But, max data length is 1000. |
kosaka | 13:ba71733c11d7 | 24 | #define TS3 0.002 // [s], timerTS3のサンプル時間(データセーブ用) sampling time (priority low: precision 4ms) |
kosaka | 13:ba71733c11d7 | 25 | #define TS4 0.2 // [s], timerTS4のサンプル時間(モニタ表示用) sampling time (priority lowest: precision 4ms) to display data to PC tera term |
kosaka | 12:459af534d1ee | 26 | //void timerTS1(void const *argument), CallTimerTS3(void const *argument), CallTimerTS4(void const *argument); |
kosaka | 12:459af534d1ee | 27 | // RtosTimer RtosTimerTS1(timerTS1); // RtosTimer priority is osPriorityAboveNormal, just one above main() |
kosaka | 12:459af534d1ee | 28 | // Thread ThreadTimerTS3(CallTimerTS3,NULL,osPriorityBelowNormal); |
kosaka | 12:459af534d1ee | 29 | // Thread ThreadTimerTS4(CallTimerTS4,NULL,osPriorityLow); |
kosaka | 13:ba71733c11d7 | 30 | #define TMAX 5.0 // [s], プログラム開始から終了までの時間 experiment starts from 0[s] to TMAX[s] |
kosaka | 13:ba71733c11d7 | 31 | #define N_DATA 1000 // PC上のmbed USB ディスクにセーブするデータの数 |
kosaka | 12:459af534d1ee | 32 | |
kosaka | 12:459af534d1ee | 33 | // 電流制御マイナーループ |
kosaka | 13:ba71733c11d7 | 34 | #define iKP 10./2 // [V/A], 電流制御PIDのPゲイン |
kosaka | 13:ba71733c11d7 | 35 | #define iKI 100./2 // [V/A s], 電流制御PIDのIゲイン |
kosaka | 13:ba71733c11d7 | 36 | #define iKD 0 // [V/A/s], 電流制御PIDのDゲイン |
kosaka | 13:ba71733c11d7 | 37 | #define vMAX 3.3 // [V], 指令電圧の最大値(超えるとこの値に制限する) |
kosaka | 12:459af534d1ee | 38 | |
kosaka | 12:459af534d1ee | 39 | // 速度制御メインループ |
kosaka | 13:ba71733c11d7 | 40 | #ifdef USE_CURRENT_CONTROL // 電流制御ありのとき |
kosaka | 13:ba71733c11d7 | 41 | #define wKp 0.05 // [A/(rad/s)], 速度制御PIDのPゲイン |
kosaka | 13:ba71733c11d7 | 42 | #define wKi 2.50 // [A/(rad/s) s], 速度制御PIDのIゲイン |
kosaka | 13:ba71733c11d7 | 43 | #define wKd 0 // [A/(rad/s)/s], 速度制御PIDのDゲイン |
kosaka | 13:ba71733c11d7 | 44 | #else // 電流制御なしのとき |
kosaka | 13:ba71733c11d7 | 45 | #define wKp 0.05 // [A/(rad/s)], 速度制御PIDのPゲイン |
kosaka | 13:ba71733c11d7 | 46 | #define wKi 0.5//2.50 // [A/(rad/s) s], 速度制御PIDのIゲイン |
kosaka | 14:02411880ffb9 | 47 | #define wKd 0.0005 // [A/(rad/s)/s], 速度制御PIDのDゲイン |
kosaka | 12:459af534d1ee | 48 | #endif |
kosaka | 13:ba71733c11d7 | 49 | #define iLPF 0.95 // 0-1, 速度に対する1次LPFの強さ; Low Pass Filter, G(z)=(1-a)/(z-a) |
kosaka | 14:02411880ffb9 | 50 | #define iMAX 1.0//3.3 // [A], 電流指令の最大値 |
kosaka | 12:459af534d1ee | 51 | |
kosaka | 13:ba71733c11d7 | 52 | #define DA_PORT p18 // デバッグ用DAポート analog out (DA) port of mbed |
kosaka | 13:ba71733c11d7 | 53 | /*********** 使用するポートやサンプル時間、制御ゲインなどの設定 (ここから) ***************/ |
kosaka | 12:459af534d1ee | 54 | |
kosaka | 12:459af534d1ee | 55 | |
kosaka | 13:ba71733c11d7 | 56 | // モータの定数、信号などの宣言 |
kosaka | 12:459af534d1ee | 57 | typedef struct struct_motor_parameters{ |
kosaka | 12:459af534d1ee | 58 | #ifdef SIMULATION // シミュレーションのとき |
kosaka | 12:459af534d1ee | 59 | float L; // [H], インダクタンス |
kosaka | 12:459af534d1ee | 60 | float R; // [Ω], モータ巻線抵抗 |
kosaka | 12:459af534d1ee | 61 | float phi; // [V s], 永久磁石の鎖交磁束 |
kosaka | 12:459af534d1ee | 62 | float Jm; // [Nms^2], イナーシャ |
kosaka | 12:459af534d1ee | 63 | float Tm; // [Nm], モータトルク |
kosaka | 12:459af534d1ee | 64 | float TL; // [Nm], 負荷トルク |
kosaka | 13:ba71733c11d7 | 65 | float p; // 極対数 |
kosaka | 12:459af534d1ee | 66 | #endif |
kosaka | 13:ba71733c11d7 | 67 | float th[2]; // [rad], モータの回転角, th[0]は現在の値, th[1]はTS0[s]だけ過去の値 |
kosaka | 13:ba71733c11d7 | 68 | float w; // [rad/s], モータの回転速度 |
kosaka | 12:459af534d1ee | 69 | float w_lpf; // [rad/s], フィルタで高周波ノイズを除去したモータ速度 |
kosaka | 13:ba71733c11d7 | 70 | float i; // [A], モータ電流 |
kosaka | 13:ba71733c11d7 | 71 | float v; // [V], モータ電圧 |
kosaka | 12:459af534d1ee | 72 | }motor_parameters; |
kosaka | 12:459af534d1ee | 73 | |
kosaka | 13:ba71733c11d7 | 74 | // 電流制御マイナーループの定数、変数の宣言 |
kosaka | 12:459af534d1ee | 75 | typedef struct struct_current_loop_parameters{ |
kosaka | 13:ba71733c11d7 | 76 | float i_ref; // iの目標値 |
kosaka | 13:ba71733c11d7 | 77 | float v_ref; // vの目標値 |
kosaka | 13:ba71733c11d7 | 78 | float eI; // 電流制御用偏差の積分値(積分項) |
kosaka | 13:ba71733c11d7 | 79 | float e_old; // 電流制御用偏差の1サンプル過去の値 |
kosaka | 12:459af534d1ee | 80 | }current_loop_parameters; |
kosaka | 12:459af534d1ee | 81 | |
kosaka | 13:ba71733c11d7 | 82 | // 速度制御メインループの定数、変数の宣言 |
kosaka | 12:459af534d1ee | 83 | typedef struct struct_velocity_loop_parameters{ |
kosaka | 12:459af534d1ee | 84 | float w_lpf; // [rad/s], モータ速度(LPF通過後) |
kosaka | 12:459af534d1ee | 85 | float w_ref; // [rad/s], モータ目標速度 |
kosaka | 12:459af534d1ee | 86 | float i_ref; // 電流指令[A] |
kosaka | 13:ba71733c11d7 | 87 | float eI; // 速度制御用偏差の積分値(積分項) |
kosaka | 13:ba71733c11d7 | 88 | float e_old; // 速度制御用偏差の1サンプル過去の値 |
kosaka | 12:459af534d1ee | 89 | }velocity_loop_parameters; |
kosaka | 12:459af534d1ee | 90 | |
kosaka | 13:ba71733c11d7 | 91 | // タイマー宣言 |
kosaka | 13:ba71733c11d7 | 92 | extern void timerTS0(); // TS0[s]ごとにコールされるタイマー timer called every TS0[s]. |
kosaka | 13:ba71733c11d7 | 93 | extern void timerTS1(void const *argument); // TS1[s]ごとにコールされるタイマー timer called every TS1[s]. |
kosaka | 13:ba71733c11d7 | 94 | extern void timerTS2(); // TS2[s]ごとにコールされるタイマー timer called every TS2[s]. |
kosaka | 13:ba71733c11d7 | 95 | extern void timerTS3(); // TS3[s]ごとにコールされるタイマー timer called every TS3[s]. |
kosaka | 13:ba71733c11d7 | 96 | extern void timerTS4(); // TS4[s]ごとにコールされるタイマー timer called every TS4[s]. |
kosaka | 12:459af534d1ee | 97 | |
kosaka | 13:ba71733c11d7 | 98 | extern void init_parameters(); // モータの機器定数等の設定, 制御器の初期化する関数の宣言 |
kosaka | 12:459af534d1ee | 99 | |
kosaka | 13:ba71733c11d7 | 100 | extern unsigned long _countTS0; // TS0[s]ごとのカウント数 |
kosaka | 13:ba71733c11d7 | 101 | extern float _time; // [s], プログラム開始時からの経過時間 |
kosaka | 12:459af534d1ee | 102 | |
kosaka | 12:459af534d1ee | 103 | extern motor_parameters p; // モータの定数、信号など |
kosaka | 12:459af534d1ee | 104 | extern current_loop_parameters il; // 電流制御マイナーループの定数、変数 |
kosaka | 12:459af534d1ee | 105 | extern velocity_loop_parameters vl; // 速度制御メインループの定数、変数 |
kosaka | 12:459af534d1ee | 106 | |
kosaka | 13:ba71733c11d7 | 107 | extern float data[][5]; // PC上のmbed USB ディスクにセーブするデータ // memory to save data offline instead of "online fprintf". |
kosaka | 13:ba71733c11d7 | 108 | extern unsigned short _countTS3_data; // data[i][5]のカウンタ |
kosaka | 12:459af534d1ee | 109 | |
kosaka | 12:459af534d1ee | 110 | #endif |