manabu kosaka / Mbed 2 deprecated DCmotor2

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 manabu kosaka

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?

UserRevisionLine numberNew 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": 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