Makoto Kobayashi
機械工学実験1作業用
Dependencies: PID QEI USBHost mbed
Revision 1:36385b59d183, committed 2015-04-20
- Comitter:
- neoqased
- Date:
- Mon Apr 20 03:34:10 2015 +0000
- Parent:
- 0:d7629adcea6d
- Commit message:
- kikaikougakujikken
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| setting.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r d7629adcea6d -r 36385b59d183 main.cpp
--- a/main.cpp Mon Oct 20 11:15:26 2014 +0000
+++ b/main.cpp Mon Apr 20 03:34:10 2015 +0000
@@ -8,8 +8,13 @@
//1回転あたりのパルス
#define PULSE_PER_REVOLUTION 200
-//PIDのウェイト ミリ秒
+//PIDのウェイト ミリ秒(制御周期)
#define PID_RATE 10
+//ギア比
+#define GEAR_RATIO 2
+
+#define KP_SCALE 10
+#define TI_SCALE 20
/**************************入出力ポート設定**************************/
DigitalOut led1(LED1); //mbed上LED出力4つ
@@ -30,8 +35,8 @@
Timer timer; //タイマー
typedef struct{ //時間tとその時のrpmを記録するための構造体
- float t;
- float rpm;
+ unsigned short t;
+ short rpm;
} result;
/**************************プロトタイプ宣言**************************/
@@ -86,7 +91,7 @@
pwm = vol;
if(i1 >= 20){
t = timer.read();
- rpm = (float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / t * 2; //減速比2
+ rpm = (float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / t * GEAR_RATIO; //減速比2
pc.printf("Duty Ratio = %.3f , %6d RPM\n", vol, (int)rpm);
encoder.reset();
i1 = 0;
@@ -99,6 +104,7 @@
}
case 2:{ //モード2はモータの応答を調べる．デューティ比1でモータを回転させ，100msごとの経過時間とRPMをUSBメモリのcsvファイルに出力する．約15秒で終了する．
//result exp2[(int)(15*1000/PID_RATE+1)] = {};
+ float dt = 0, tnow = 0, tpre = 0;
//初期化
exp[0].t = 0;
exp[0].rpm = 0;
@@ -109,8 +115,11 @@
//ループ(約15秒で終了)
for(int i = 0; i < 15*1000/PID_RATE; i++){ //10msごとに時間とrpmの取得
wait_ms(PID_RATE);
- exp[i+1].t = timer.read();
- exp[i+1].rpm = (float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / (exp[i+1].t - exp[i].t) * 2; //減速比2
+ tpre = tnow;
+ tnow = timer.read();
+ exp[i+1].t = (unsigned short)(tnow * 1000);
+ dt = tnow - tpre;
+ exp[i+1].rpm = (short)((float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / dt * GEAR_RATIO); //減速比2
encoder.reset();
}
for(int i = 100; i >= 0; i--){ //ゆるやかに減速
@@ -126,7 +135,7 @@
}
fprintf(fp,"Time , RPM\n");
for(int i = 0; i <= 15*1000/PID_RATE; i++){
- fprintf(fp,"%f , %f\n", exp[i].t, exp[i].rpm); //ファイルに実験データの書き込み
+ fprintf(fp,"%f , %d\n", (float)exp[i].t / 1000.0, exp[i].rpm); //ファイルに実験データの書き込み
}
fclose(fp);
Buzzer(4); //終了を知らせる
@@ -134,7 +143,7 @@
}
case 3:{ //モード3はモータの応答を調べる．PID制御をかけ，経過時間とRPMをUSBメモリのcsvファイルに出力する．約15秒で終了する．
//result exp3[(int)(15*1000/PID_RATE+1)] = {};
- float kp = 0, ki = 0, kd = 0, max_rpm = 0, target_rpm = 0, reduction = 0;
+ float kp = 0, ti = 0, td = 0, max_rpm = 0, target_rpm = 0, reduction = 0, dt = 0, tnow = 0, tpre = 0, rpmnow = 0;
//int pulse[15*1000/PID_RATE+2] = {};
//int j = 0;
//float times = 0;
@@ -179,13 +188,13 @@
//fscanf(fppid,"%f",&target_rpm);
//fclose(fppid);
- kp = KP;
- ki = KI;
- kd = KD;
+ kp = KP / KP_SCALE;
+ ti = TI * TI_SCALE;
+ td = TD;
max_rpm = MAX_RPM;
target_rpm = TARGET_RPM;
- PID pid(kp, ki, kd, PID_RATE); //PIDの設定
+ PID pid(kp, ti, td, PID_RATE); //PIDの設定
pid.setInputLimits(0.0, max_rpm);
pid.setOutputLimits(0.0, 1.0);
pid.setMode(AUTO_MODE);
@@ -195,9 +204,13 @@
exp[0].rpm = 0;
timer.start();
for(int i = 0; i < 15*1000/PID_RATE; i++){ //15秒間実行する
- exp[i+1].t = timer.read();
- exp[i+1].rpm = (float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / (exp[i+1].t - exp[i].t) * 2;
- pid.setProcessValue(exp[i+1].rpm);
+ tpre = tnow;
+ tnow = timer.read();
+ exp[i+1].t = (unsigned short)(tnow * 1000);
+ dt = tnow - tpre;
+ rpmnow = (float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / dt * GEAR_RATIO;
+ exp[i+1].rpm = (short)rpmnow;
+ pid.setProcessValue(rpmnow);
pwm = pid.compute();
encoder.reset();
//pulse[i+1] = encoder.getPulses() / 2.0;
@@ -222,10 +235,10 @@
Buzzer(-1); //エラーコード-1のブザーを鳴らす
break; //終了
}
- fprintf(fp,"kp = %f ki = %f kd = %f\n",kp, ki, kd);
+ fprintf(fp,"kp = %.6f ki = %.6f kd = %.6f\n", (float)KP, (float)TI, (float)TD);
fprintf(fp,"Time , RPM\n");
for(int i = 0; i <= 15*1000/PID_RATE; i++){
- fprintf(fp,"%f , %f\n", exp[i].t, exp[i].rpm); //ファイルに実験データの書き込み
+ fprintf(fp,"%f , %d\n", (float)exp[i].t / 1000.0, exp[i].rpm); //ファイルに実験データの書き込み
}
fclose(fp);
Buzzer(5); //終了を知らせる
diff -r d7629adcea6d -r 36385b59d183 setting.h --- a/setting.h Mon Oct 20 11:15:26 2014 +0000 +++ b/setting.h Mon Apr 20 03:34:10 2015 +0000 @@ -1,10 +1,10 @@ #ifndef _SETTING_H #define _SETTING_H -#define KP 0.00135 -#define KI 0.00962 -#define KD 0.0 +#define KP 50 +#define TI 1 +#define TD 0.0 #define TARGET_RPM 4000 -#define MAX_RPM 10000 +#define MAX_RPM 7550 #endif \ No newline at end of file