自動投射のみです。(手動用にチューニングしました)

Dependencies:   HCSR04 PID QEI mbed

Fork of Lets_move_Seki2018_ver2 by INCTRC Information Sharing Test

Revision:
2:c32991ba628f
Parent:
1:62b321f6c9c3
Child:
3:1223cab367d9
diff -r 62b321f6c9c3 -r c32991ba628f main.cpp
--- a/main.cpp	Sun Jul 15 23:56:24 2018 +0000
+++ b/main.cpp	Sat Aug 18 04:59:28 2018 +0000
@@ -1,5 +1,5 @@
 /* タイマ割り込みを使用して回転数(RPS)を取得し表示するプログラム */
-/* これまでの計算手順では回転数が取得できないことが判明し、改善済である */
+/* 吉田啓人氏が頑張って改善したMDプログラム用に改善しました */
 /* 具体的には取得したパルス数を分解能で割り算→掛け算でRPSへ変換から */
 /* 取得したパルス数を掛け算→分解能で割ってRPSへ変換としている。 */
 /* ロリコンにはTIM2~TIM5がいいらしい(Nucleo_F401re) */
@@ -18,12 +18,13 @@
 #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 RF(0.5, 0.4, 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);
+Serial photo(D8, D2);
 DigitalOut emergency(PA_13);
 DigitalOut myled(D13);
 
@@ -169,10 +170,11 @@
     */
     
     //pc.printf("RF: %d   RB: %d  LF: %d  LB: %d\n", rpm_RF, rpm_RB, rpm_LF, rpm_LB);
-    //pc.printf("%d\n", rpm_RF);
+    pc.printf("%d\n", rpm_LF);
     //pc.printf("%lf\n", n_v);
     //pc.printf("%lf\n", h_v);
     //pc.printf("rpm: %d  n_v: %lf\n", rpm[1], n_v);
+    //pc.printf("%lf\n", timer.read());
 
     wheel_RF.reset();
     wheel_RB.reset();
@@ -189,20 +191,19 @@
     //pc.printf("rpm: %d  a_v: %lf\n", rpm[1], a_v);
 }
 
-void front_PID()
+int front_PID(int setpoint)
 { 
         // センサ出力値の最小、最大 
-        //RF.setInputLimits(0, 1100);
-        RF.setInputLimits(-400, 400);
-        RB.setInputLimits(-400, 400);
-        LF.setInputLimits(-400, 400);
-        LB.setInputLimits(-400, 400);
+        RF.setInputLimits(-2000, 2000);
+        RB.setInputLimits(-2000, 2000);
+        LF.setInputLimits(-2000, 2000);
+        LB.setInputLimits(-2000, 2000);
         
         // 制御量の最小、最大 
-        RF.setOutputLimits(0x01, 0x37);
-        RB.setOutputLimits(0x01, 0x37);
-        LF.setOutputLimits(0x01, 0x37);
-        LB.setOutputLimits(0x01, 0x37);
+        RF.setOutputLimits(0x0C, 0x7C);
+        RB.setOutputLimits(0x0C, 0x7C);
+        LF.setOutputLimits(0x0C, 0x7C);
+        LB.setOutputLimits(0x0C, 0x7C);
 
         // よくわからんやつ 
         RF.setMode(AUTO_MODE);
@@ -216,10 +217,10 @@
         //LB: +
         
         // 目標値 
-        RF.setSetPoint(300);
-        RB.setSetPoint(300);
-        LF.setSetPoint(300);
-        LB.setSetPoint(300);
+        RF.setSetPoint(setpoint);
+        RB.setSetPoint(setpoint);
+        LF.setSetPoint(setpoint);
+        LB.setSetPoint(setpoint);
         
         // センサ出力 
         RF.setProcessValue(rpm_RF);
@@ -234,26 +235,27 @@
         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];
+        true_RF_data[0] = 0x7D - RF_data[0];
+        true_RB_data[0] = 0x7D - RB_data[0];
+        true_LF_data[0] = 0x7D - LF_data[0];
+        true_LB_data[0] = 0x7D - LB_data[0];
+        
+        return 0;
 }
 
-void back_PID()
+int back_PID(int setpoint)
 { 
         // センサ出力値の最小、最大 
-        //RF.setInputLimits(0, 1100);
-        RF.setInputLimits(-400, 400);
-        RB.setInputLimits(-400, 400);
-        LF.setInputLimits(-400, 400);
-        LB.setInputLimits(-400, 400);
+        RF.setInputLimits(0, 2000);
+        RB.setInputLimits(0, 2000);
+        LF.setInputLimits(0, 2000);
+        LB.setInputLimits(0, 2000);
         
         // 制御量の最小、最大 
-        RF.setOutputLimits(0x48, 0x7D);
-        RB.setOutputLimits(0x48, 0x7D);
-        LF.setOutputLimits(0x48, 0x7D);
-        LB.setOutputLimits(0x48, 0x7D);
+        RF.setOutputLimits(0x84, 0xFF);
+        RB.setOutputLimits(0x84, 0xFF);
+        LF.setOutputLimits(0x84, 0xFF);
+        LB.setOutputLimits(0x84, 0xFF);
 
         // よくわからんやつ 
         RF.setMode(AUTO_MODE);
@@ -262,16 +264,17 @@
         LB.setMode(AUTO_MODE);
         
         // 目標値 
-        RF.setSetPoint(300);
-        RB.setSetPoint(300);
-        LF.setSetPoint(300);
-        LB.setSetPoint(300);
+        RF.setSetPoint(setpoint);
+        RB.setSetPoint(setpoint);
+        LF.setSetPoint(setpoint);
+        LB.setSetPoint(setpoint);
         
         // センサ出力 
-        RF.setProcessValue(rpm_RF);
-        RB.setProcessValue(rpm_RB);
-        LF.setProcessValue(rpm_LF);
-        LB.setProcessValue(rpm_LB);
+        //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
+        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();
@@ -279,19 +282,13 @@
         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];
-        */
+        return 0;
 }
 
 int main(void)
 {   
     emergency = 0;
-    send_data[0] = 0x40;
+    send_data[0] = 0x80;
     i2c.write(0xA0, send_data, 1);
     i2c.write(0xA2, send_data, 1);
     i2c.write(0xA4, send_data, 1);
@@ -307,66 +304,41 @@
 
     while(1)
     {  
-        timer.start();
-        myled = 1;
+        /*
+        //後進PID確認
+        back_PID(200);
+        i2c.write(0x20, LF_data, 1, false);
+        wait_us(20);
+        */
         
+        timer.start();        
         while(timer.read() <= 5.0f) {
-            //timer.start();
-            
-            front_PID();
-            /*
-            ice(0xA0, 0x01);
-            ice(0xA2, 0x02);
-            ice(0xA4, 0x03);
-            ice(0xA6, 0x04);
-            */
-            i2c.write(0xA0, true_RF_data, 1, false);     
-            wait_us(20);       
-            
-            i2c.write(0xA2, true_RB_data, 1, false);
-            wait_us(20);
-            
-            i2c.write(0xA4, true_LB_data, 1, false);
-            wait_us(20);
-            
-            i2c.write(0xA6, true_LF_data, 1, false);
+            myled = 1;
+            front_PID(200);
+            i2c.write(0x20, true_LF_data, 1, false);
             wait_us(20);
-            //wait_ms(50);
         }
-        
-        timer.reset();
-        myled = 0;
-        wait(1);
-        
-        RF.reset();
-        RB.reset();
         LF.reset();
-        LB.reset();
-        wait(1);
-        
-        //wait(5);
-        
-        /*
-        timer.start();
-        while(timer.read() <= 5.0f) {
-            //timer.start();
-            back_PID();
-            i2c.write(0xA0, RF_data, 1);
-            i2c.write(0xA2, RB_data, 1);
-            i2c.write(0xA4, LB_data, 1);
-            i2c.write(0xA6, LF_data, 1);
-            wait_ms(40);
+        while((timer.read() > 5.0f) && (timer.read() <= 10.0f)) {
+            myled = 0;
+            true_LF_data[0] = 0x80;
+            i2c.write(0x20, true_LF_data, 1, false);
+            wait_us(20);
+        }
+        while((timer.read() > 10.0f) && (timer.read() <= 15.0f)) {
+            myled = 1;
+            back_PID(200);
+            i2c.write(0x20, LF_data, 1, false);
+            wait_us(20);
+        }
+        LF.reset();
+        while((timer.read() > 15.0f) && (timer.read() <= 20.0f)) {
+            myled = 0;
+            true_LF_data[0] = 0x80;
+            i2c.write(0x20, true_LF_data, 1, false);
+            wait_us(20);
         }
         timer.reset();
-        */
-        /*
-        RF.reset();
-        RB.reset();
-        LF.reset();
-        LB.reset();
-        //wait(5);
-        */
-        
         
         /*
         // センサ出力値の最小、最大 
@@ -413,52 +385,35 @@
         */
         
         /*
-        // センサ出力値の最小、最大 
-        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]--){
+        //どんどん加速(逆転)
+        for(send_data[0] = 0x81; send_data[0] < 0xF5; 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);
+            
+            i2c.write(0x20, send_data, 1);
             wait(0.1);
         }
-        
-        //だんだん減速
-        for(send_data[0] = 0x02; send_data[0] <= 0x37; send_data[0]++){
+        //だんだん減速(逆転)
+        for(send_data[0] = 0xF4; send_data[0] > 0x80; 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);
+            
+            i2c.write(0x20, send_data, 1);
             wait(0.1);
         }
+        send_data[0] = 0x80;
+        i2c.write(0x20, send_data, 1);
+        wait(5);
         */
     }
 }