2019NHK_teamA / Mbed 2 deprecated 2019_A_ver8

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed QEI PID

Files at this revision

API Documentation at this revision

Revision 0:f73c1b076ae4, committed 2018-07-10

Comitter:
yuron
Date:
Tue Jul 10 13:40:10 2018 +0000
Child:
1:62b321f6c9c3
Commit message:
B??????????????????(2018/07/10??); PID???????????????????

Changed in this revision

PID.lib Show annotated file Show diff for this revision Revisions of this file
QEI.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Tue Jul 10 13:40:10 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aberk/code/PID/#6e12a3e5af19

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Tue Jul 10 13:40:10 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jul 10 13:40:10 2018 +0000
@@ -0,0 +1,331 @@
+/* タイマ割り込みを使用して回転数(RPS)を取得し表示するプログラム */
+/* これまでの計算手順では回転数が取得できないことが判明し、改善済である */
+/* 具体的には取得したパルス数を分解能で割り算→掛け算でRPSへ変換から */
+/* 取得したパルス数を掛け算→分解能で割ってRPSへ変換としている。 */
+/* ロリコンにはTIM2~TIM5がいいらしい(Nucleo_F401re) */
+/* TIM2(CH1: NC, CH2: D3(PB_3), CH3: D6(PB_10), CH4: NC) */
+/* TIM3(CH1: D5(PB_4), CH2: D9(PC_7), CH3: NC, CH4: NC) */
+/* TIM4(CH1: D10(PB_6), CH2: NC, CH3: NC, CH4: NC) */
+/* TIM5(CH1: NC, CH2: NC, CH3: NC, CH4: NC) */
+/* 2018/6/9追記 */
+/* ついに回転数の！PI制御を実現できました。 */
+/* タイマ割込みで角速度を取得してみようと思います。 */
+
+#include "mbed.h"
+#include "math.h"
+#include "QEI.h"
+#include "PID.h"
+#define PI  3.14159265359
+
+PID controller(0.7, 0.7, 0.0, 0.001);
+PID RF(0.5, 0.3, 0.0, 0.001);
+PID RB(0.5, 0.3, 0.0, 0.001);
+PID LF(0.5, 0.3, 0.0, 0.001);
+PID LB(0.5, 0.3, 0.0, 0.001);
+I2C i2c(PB_3, PB_10);  //SDA, SCL
+//Serial pc(USBTX, USBRX);
+DigitalOut emergency(PA_13);
+
+/* 以下446基板用 */
+QEI wheel_LB(PA_1, PA_0, NC, 624);
+QEI wheel_RB(PB_6, PB_7, NC, 624);
+QEI wheel_LF(PB_4, PB_5, NC, 624);
+QEI wheel_RF(PC_8, PC_9, NC, 624);
+
+Ticker get_RPS;
+Ticker print_RPS;
+
+int rps_RF;
+int rps_RB;
+int rps_LF;
+int rps_LB;
+
+int rpm_RF;
+int rpm_RB;
+int rpm_LF;
+int rpm_LB;
+
+int pulse_RF;
+int pulse_RB;
+int pulse_LF;
+int pulse_LB;
+
+int counter_RF;
+int counter_RB;
+int counter_LF;
+int counter_LB;
+
+double a_v; /* 角速度 */
+double n_v; /* 速度(秒速) */ 
+double h_v; /* 速度(時速) */ 
+double n_a; /* 加速度 */
+
+char send_data[1];
+char true_send_data[1];
+
+char RF_data[1];
+char RB_data[1];
+char LF_data[1];
+char LB_data[1];
+char true_RF_data[1];
+char true_RB_data[1];
+char true_LF_data[1];
+char true_LB_data[1];
+
+/* ロリコン処理関数 */
+void flip();
+/* RPS表示関数 */
+void flip2();
+
+void flip(){
+    
+    pulse_RF = wheel_RF.getPulses();
+    pulse_RB = wheel_RB.getPulses();
+    pulse_LF = wheel_LF.getPulses();
+    pulse_LB = wheel_LB.getPulses();
+        
+    /* *rps変換 */
+    /*10ms*100 = 1s
+    counter_RB = pulse_RB * 100;
+    counter_LB = pulse_LB * 100;
+    */
+    
+    //40ms*25 = 1s
+    counter_RF = pulse_RF * 25;
+    counter_RB = pulse_RB * 25;
+    counter_LF = pulse_LF * 25;
+    counter_LB = pulse_LB * 25;
+    
+    /*
+    //100ms*10 = 1s
+    counter_RB = pulse_RB * 10;
+    counter_LB = pulse_LB * 10;
+    */
+
+    /* /分解能 */
+    rps_RF = counter_RF / 100;
+    rps_RB = counter_RB / 100;
+    rps_LF = counter_LF / 100;
+    rps_LB = counter_LB / 100;
+    
+    rpm_RF = pulse_RF * 25 * 60 / 100;
+    rpm_RB = pulse_RB * 25 * 60 / 100;
+    rpm_LF = pulse_LF * 25 * 60 / 100;
+    rpm_LB = pulse_LB * 25 * 60 / 100;
+    
+    /*
+    rps[0] = counter_RB / 100;
+    rps[1] = counter_LB / 100;
+    
+    rpm[0] = pulse_RB * 25 * 60 / 100;
+    rpm[1] = pulse_LB * 25 * 60 / 100;
+    */
+    
+    /* RPMから角速度へ変換 */
+    a_v = rpm_LB * PI / 30;
+    
+    /* RPMから速度(秒速)へ変換 */
+    /* タイヤ半径を0.05[m]とする */
+    n_v = 0.05 * 2 * PI * rpm_LB / 60;
+    
+    /* 速度(秒速)から速度(時速)へ変換 */
+    h_v = n_v * 3600 / 1000;
+    
+    //n_a = n_v / 
+    
+    /*
+    if(rpm[1] < 200){
+        send_data[0]--;
+    }
+    else if(rpm[1] > 250){
+        send_data[1]++;
+    }
+    */
+    
+    //pc.printf("RF: %d   RB: %d  LF: %d  LB: %d\n", rpm_RF, rpm_RB, rpm_LF, rpm_LB);
+    //pc.printf("%d\n", abs(rpm_RF));
+    //pc.printf("%lf\n", n_v);
+    //pc.printf("%lf\n", h_v);
+    //pc.printf("rpm: %d  n_v: %lf\n", rpm[1], n_v);
+
+    wheel_RF.reset();
+    wheel_RB.reset();
+    wheel_LF.reset();
+    wheel_LB.reset();
+}
+
+void flip2()
+{
+    //pc.printf("RPS_RB: %d   RPS_LB: %d\n", abs(rps[0]), abs(rps[1]));
+    //pc.printf("RPM_RB: %d   RPM_LB: %d\n", abs(rpm[0]), abs(rpm[1]));
+    //pc.printf("%d\n", rpm[1]);
+    //pc.printf("%lf\n", a_v);
+    //pc.printf("rpm: %d  a_v: %lf\n", rpm[1], a_v);
+}
+
+void PID()
+{ 
+        // センサ出力値の最小、最大 
+        //RF.setInputLimits(0, 1100);
+        RF.setInputLimits(-400, 400);
+        RB.setInputLimits(-400, 400);
+        LF.setInputLimits(-400, 400);
+        LB.setInputLimits(-400, 400);
+        
+        // 制御量の最小、最大 
+        RF.setOutputLimits(0x01, 0x37);
+        RB.setOutputLimits(0x01, 0x37);
+        LF.setOutputLimits(0x01, 0x37);
+        LB.setOutputLimits(0x01, 0x37);
+
+        // よくわからんやつ 
+        RF.setMode(AUTO_MODE);
+        RB.setMode(AUTO_MODE);
+        LF.setMode(AUTO_MODE);
+        LB.setMode(AUTO_MODE);
+        
+        // 目標値 
+        RF.setSetPoint(300);
+        RB.setSetPoint(300);
+        LF.setSetPoint(300);
+        LB.setSetPoint(300);
+        
+        // センサ出力 
+        RF.setProcessValue(abs(rpm_RF));
+        RB.setProcessValue(abs(rpm_RB));
+        LF.setProcessValue(abs(rpm_LF));
+        LB.setProcessValue(abs(rpm_LB));
+        
+        // 制御量(計算結果) 
+        RF_data[0] = RF.compute();
+        RB_data[0] = RB.compute();
+        LF_data[0] = LF.compute();
+        LB_data[0] = LB.compute();
+       
+        // 制御量をPWM値に変換 
+        true_RF_data[0] = 0x38 - RF_data[0];
+        true_RB_data[0] = 0x38 - RB_data[0];
+        true_LF_data[0] = 0x38 - LF_data[0];
+        true_LB_data[0] = 0x38 - LB_data[0];
+}
+
+int main(void)
+{   
+    emergency = 0;
+    send_data[0] = 0x40;
+    i2c.write(0xA0, send_data, 1);
+    i2c.write(0xA2, send_data, 1);
+    i2c.write(0xA4, send_data, 1);
+    i2c.write(0xA6, send_data, 1);
+    wait(0.1);
+    
+    /* 加減速処理を入れた場合処理サイクルの最小周期は40msであった(2018/5/12) */
+    get_RPS.attach_us(&flip, 40000);
+    //get_RPS.attach_us(&flip, 100000);
+
+    //RPS表示(0.1sサイクル)
+    //print_RPS.attach(&flip2, 0.1);
+
+    while(1)
+    {  
+        
+        PID();
+        i2c.write(0xA0, true_RF_data, 1);
+        i2c.write(0xA2, true_RB_data, 1);
+        i2c.write(0xA4, true_LB_data, 1);
+        i2c.write(0xA6, true_LF_data, 1);
+        
+        /*
+        // センサ出力値の最小、最大 
+        RF.setInputLimits(0, 1100);
+        RB.setInputLimits(0, 1100);
+        LF.setInputLimits(0, 1100);
+        LB.setInputLimits(0, 1100);
+        
+        // 制御量の最小、最大 
+        RF.setOutputLimits(0x01, 0x37);
+        RB.setOutputLimits(0x01, 0x37);
+        LF.setOutputLimits(0x01, 0x37);
+        LB.setOutputLimits(0x01, 0x37);
+
+        // よくわからんやつ 
+        RF.setMode(AUTO_MODE);
+        RB.setMode(AUTO_MODE);
+        LF.setMode(AUTO_MODE);
+        LB.setMode(AUTO_MODE);
+        
+        // 目標値 
+        RF.setSetPoint(400);
+        RB.setSetPoint(400);
+        LF.setSetPoint(400);
+        LB.setSetPoint(400);
+        
+        // センサ出力 
+        RF.setProcessValue(abs(rpm_RF));
+        RB.setProcessValue(abs(rpm_RB));
+        LF.setProcessValue(abs(rpm_LF));
+        LB.setProcessValue(abs(rpm_LB));
+        
+        // 制御量(計算結果) 
+        RF_data[0] = RF.compute();
+        RB_data[0] = RB.compute();
+        LF_data[0] = LF.compute();
+        LB_data[0] = LB.compute();
+       
+        // 制御量をPWM値に変換 
+        true_RF_data[0] = 0x38 - RF_data[0];
+        true_RB_data[0] = 0x38 - RB_data[0];
+        true_LF_data[0] = 0x38 - LF_data[0];
+        true_LB_data[0] = 0x38 - LB_data[0];
+        */
+        
+        /*
+        // センサ出力値の最小、最大 
+        controller.setInputLimits(0, 1100);
+        
+        // 制御量の最小、最大 
+        controller.setOutputLimits(0x01, 0x37);
+        
+        // よくわからんやつ 
+        controller.setMode(AUTO_MODE);
+        
+        // 目標値 
+        controller.setSetPoint(400);
+        
+        // センサ出力 
+        controller.setProcessValue(abs(rpm[1]));
+        
+        // 制御量(計算結果) 
+        send_data[0] = controller.compute();
+        
+        // 制御量をPWM値に変換 
+        true_send_data[0] = 0x38 - send_data[0];
+        
+        i2c.write(0x88, true_send_data, 1);
+        */
+        
+        /*
+        //どんどん加速
+        for(send_data[0] = 0x37; send_data[0] > 0x01; send_data[0]--){
+            //ice(0x88, send_data);
+            //ice(0xA2, send_data);
+            i2c.write(0xA0, send_data, 1);
+            i2c.write(0xA2, send_data, 1);
+            i2c.write(0xA4, send_data, 1);
+            i2c.write(0xA6, send_data, 1);
+            wait(0.1);
+        }
+        
+        //だんだん減速
+        for(send_data[0] = 0x02; send_data[0] <= 0x37; send_data[0]++){
+            //ice(0x88, send_data);
+            //ice(0xA2, send_data);
+            i2c.write(0xA0, send_data, 1);
+            i2c.write(0xA2, send_data, 1);
+            i2c.write(0xA4, send_data, 1);
+            i2c.write(0xA6, send_data, 1);
+            wait(0.1);
+        }
+        */
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Jul 10 13:40:10 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/a7c7b631e539
\ No newline at end of file