Diff: main.cpp

Revision:

1:36385b59d183

Parent:

0:d7629adcea6d

--- 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);          //終了を知らせる