Diff: main.cpp

Revision:

0:f73c1b076ae4

Child:

1:62b321f6c9c3

--- /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);
+        }
+        */
+    }
+}