機械工学実験1作業用

Dependencies:   PID QEI USBHost mbed

Files at this revision

API Documentation at this revision

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