controller.h

00001 #ifndef __controller_h
00002 #define __controller_h
00003 
00004 #define PI 3.14159265358979 // 円周率πの定義
00005 
00006 /*********** 使用するポート、サンプル時間、制御ゲインなどの設定 (ここから) ***************/
00007 //#define SIMULATION          // ブラシ付DCモータの特性をシミュレーションするとき、コメント外す
00008 //#define USE_CURRENT_CONTROL // 電流制御ありのとき、コメント外す   Current control on. Comment if current control off.
00009 
00010 //#define DEADZONE_PLUS   0//1.  // deadzone of plus side
00011 //#define DEADZONE_MINUS  0//-1.5 // deadzone of minus side
00012 
00013     // エンコーダの設定
00014 #define N_ENC   (24*4)      // エンコーダ分解能（4逓倍）＝1回転分のパルス数×４    "*4": QEI::X4_ENCODING. Number of pulses in one revolution(=360 deg) of rotary encoder.
00015 #define CH_A    p29         // A相用ポート   A phase port
00016 #define CH_B    p30         // B相用ポート   A phase port
00017 
00018     // タイマーのサンプル周期等
00019 //#define PWM_FREQ 10000.0   // [Hz], PWMチョッピング周波数  pwm freq. (> 1/(DEAD_TIME*10))
00020 //#define DEADTIME 0.0001    // [s], デッドタイム（モータに加える電圧の正負が変わるときに上下アーム短絡を避けるために同時オフする時間）  // [s], deadtime to be set between plus volt. to/from minus
00021 #define TS0     0.002      // [s], timerTS0のサンプル時間（電流制御用）   sampling time (priority highest: Ticker IRQ) of motor current i control PID using timer interrupt
00022 #define TS1     0.002      // [s], timerTS1のサンプル時間（位置制御用）   sampling time (priority high: RtosTimer) of motor angle th PID using rtos-timer
00023 #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.
00024 #define TS3     0.002      // [s], timerTS3のサンプル時間（データセーブ用） sampling time (priority low: precision 4ms)
00025 #define TS4     0.2        // [s], timerTS4のサンプル時間（モニタ表示用）  sampling time (priority lowest: precision 4ms)  to display data to PC tera term
00026 //void    timerTS1(void const *argument), CallTimerTS3(void const *argument), CallTimerTS4(void const *argument);
00027 //    RtosTimer RtosTimerTS1(timerTS1);  // RtosTimer priority is osPriorityAboveNormal, just one above main()
00028 //    Thread ThreadTimerTS3(CallTimerTS3,NULL,osPriorityBelowNormal);
00029 //    Thread ThreadTimerTS4(CallTimerTS4,NULL,osPriorityLow);
00030 #define TMAX    5.0          // [s], プログラム開始から終了までの時間   experiment starts from 0[s] to TMAX[s]
00031 #define N_DATA  1000        // PC上のmbed USB ディスクにセーブするデータの数
00032 
00033     // 電流制御マイナーループ
00034 #define iKP    10./2     // [V/A], 電流制御PIDのPゲイン
00035 #define iKI    100./2    // [V/A s], 電流制御PIDのIゲイン
00036 #define iKD    0         // [V/A/s], 電流制御PIDのDゲイン
00037 #define vMAX   3.3       // [V], 指令電圧の最大値（超えるとこの値に制限する）
00038 
00039     // 速度制御メインループ
00040 #ifdef USE_CURRENT_CONTROL  // 電流制御ありのとき
00041  #define wKp 0.05               // [A/(rad/s)], 速度制御PIDのPゲイン
00042  #define wKi 2.50               // [A/(rad/s) s], 速度制御PIDのIゲイン
00043  #define wKd 0                  // [A/(rad/s)/s], 速度制御PIDのDゲイン
00044 #else                       // 電流制御なしのとき
00045  #define wKp 0.05               // [A/(rad/s)], 速度制御PIDのPゲイン
00046  #define wKi 0.5//2.50          // [A/(rad/s) s], 速度制御PIDのIゲイン
00047  #define wKd 0.0005                // [A/(rad/s)/s], 速度制御PIDのDゲイン
00048 #endif
00049 #define iLPF   0.95         // 0-1, 速度に対する1次LPFの強さ; Low Pass Filter, G(z)=(1-a)/(z-a)
00050 #define iMAX   1.0//3.3          // [A], 電流指令の最大値
00051 
00052 #define DA_PORT p18         // デバッグ用DAポート   analog out (DA) port of mbed
00053 /*********** 使用するポートやサンプル時間、制御ゲインなどの設定 (ここから) ***************/
00054 
00055 
00056     // モータの定数、信号などの宣言
00057 typedef struct struct_motor_parameters{
00058  #ifdef SIMULATION // シミュレーションのとき
00059     float  L;      // [H], インダクタンス
00060     float  R;      // [Ω], モータ巻線抵抗
00061     float  phi;    // [V s], 永久磁石の鎖交磁束
00062     float  Jm;     // [Nms^2], イナーシャ
00063     float  Tm;     // [Nm], モータトルク
00064     float  TL;     // [Nm], 負荷トルク
00065     float  p;      // 極対数
00066  #endif
00067     float  th[2];  // [rad], モータの回転角, th[0]は現在の値, th[1]はTS0[s]だけ過去の値
00068     float  w;      // [rad/s], モータの回転速度
00069     float  w_lpf;  // [rad/s], フィルタで高周波ノイズを除去したモータ速度
00070     float  i;      // [A], モータ電流
00071     float  v;      // [V], モータ電圧
00072 }motor_parameters;
00073 
00074     // 電流制御マイナーループの定数、変数の宣言
00075 typedef struct struct_current_loop_parameters{
00076     float  i_ref;  // iの目標値
00077     float  v_ref;  // vの目標値
00078     float  eI;     // 電流制御用偏差の積分値（積分項）
00079     float  e_old;  // 電流制御用偏差の1サンプル過去の値
00080 }current_loop_parameters;
00081 
00082     // 速度制御メインループの定数、変数の宣言
00083 typedef struct struct_velocity_loop_parameters{
00084     float  w_lpf;          // [rad/s], モータ速度(LPF通過後)
00085     float  w_ref;          // [rad/s], モータ目標速度
00086     float  i_ref;          // 電流指令[A]
00087     float  eI;             // 速度制御用偏差の積分値（積分項）
00088     float  e_old;          // 速度制御用偏差の1サンプル過去の値
00089 }velocity_loop_parameters;
00090 
00091     // タイマー宣言
00092 extern void timerTS0();    // TS0[s]ごとにコールされるタイマー   timer called every TS0[s].
00093 extern void timerTS1(void const *argument);    // TS1[s]ごとにコールされるタイマー   timer called every TS1[s].
00094 extern void timerTS2();    // TS2[s]ごとにコールされるタイマー   timer called every TS2[s].
00095 extern void timerTS3();    // TS3[s]ごとにコールされるタイマー   timer called every TS3[s].
00096 extern void timerTS4();    // TS4[s]ごとにコールされるタイマー   timer called every TS4[s].
00097 
00098 extern void init_parameters();    // モータの機器定数等の設定, 制御器の初期化する関数の宣言
00099 
00100 extern unsigned long _countTS0;   // TS0[s]ごとのカウント数
00101 extern float   _time;          // [s], プログラム開始時からの経過時間
00102 
00103 extern motor_parameters            p;  // モータの定数、信号など
00104 extern current_loop_parameters     il; // 電流制御マイナーループの定数、変数
00105 extern velocity_loop_parameters    vl; // 速度制御メインループの定数、変数
00106 
00107 extern float data[][5];             // PC上のmbed USB ディスクにセーブするデータ   // memory to save data offline instead of "online fprintf".
00108 extern unsigned short _countTS3_data;  // data[i][5]のカウンタ
00109 
00110 #endif