Diff: main.cpp

Revision:

1:62b321f6c9c3

Parent:

0:f73c1b076ae4

Child:

2:c32991ba628f

--- a/main.cpp	Tue Jul 10 13:40:10 2018 +0000
+++ b/main.cpp	Sun Jul 15 23:56:24 2018 +0000
@@ -23,8 +23,9 @@
 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 pc(USBTX, USBRX);
 DigitalOut emergency(PA_13);
+DigitalOut myled(D13);
 
 /* 以下446基板用 */
 QEI wheel_LB(PA_1, PA_0, NC, 624);
@@ -34,6 +35,7 @@
 
 Ticker get_RPS;
 Ticker print_RPS;
+Timer timer;
 
 int rps_RF;
 int rps_RB;
@@ -77,6 +79,30 @@
 /* RPS表示関数 */
 void flip2();
 
+/*
+void ice(int address, int data)
+{
+    //周波数: 100kHz
+    //i2c.frequency(100000);
+    
+    // スタートコンディション出力 
+    i2c.start();
+    wait_us(20);
+    
+    // アドレスの書き込み    
+    i2c.write(address);
+    wait_us(20);
+    
+    // データの書き込み 
+    i2c.write(data);
+    wait_us(20);
+    
+    // ストップコンディション出力 
+    i2c.stop();
+    wait_us(90);
+}
+*/
+
 void flip(){
     
     pulse_RF = wheel_RF.getPulses();
@@ -143,7 +169,7 @@
     */
     
     //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("%d\n", 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);
@@ -158,12 +184,12 @@
 {
     //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("%d\n", rpm_RF);
     //pc.printf("%lf\n", a_v);
     //pc.printf("rpm: %d  a_v: %lf\n", rpm[1], a_v);
 }
 
-void PID()
+void front_PID()
 { 
         // センサ出力値の最小、最大 
         //RF.setInputLimits(0, 1100);
@@ -184,6 +210,11 @@
         LF.setMode(AUTO_MODE);
         LB.setMode(AUTO_MODE);
         
+        //RF: -
+        //RB: -
+        //LF: +
+        //LB: +
+        
         // 目標値 
         RF.setSetPoint(300);
         RB.setSetPoint(300);
@@ -191,10 +222,10 @@
         LB.setSetPoint(300);
         
         // センサ出力 
-        RF.setProcessValue(abs(rpm_RF));
-        RB.setProcessValue(abs(rpm_RB));
-        LF.setProcessValue(abs(rpm_LF));
-        LB.setProcessValue(abs(rpm_LB));
+        RF.setProcessValue(rpm_RF);
+        RB.setProcessValue(rpm_RB);
+        LF.setProcessValue(rpm_LF);
+        LB.setProcessValue(rpm_LB);
         
         // 制御量(計算結果) 
         RF_data[0] = RF.compute();
@@ -209,6 +240,54 @@
         true_LB_data[0] = 0x38 - LB_data[0];
 }
 
+void back_PID()
+{ 
+        // センサ出力値の最小、最大 
+        //RF.setInputLimits(0, 1100);
+        RF.setInputLimits(-400, 400);
+        RB.setInputLimits(-400, 400);
+        LF.setInputLimits(-400, 400);
+        LB.setInputLimits(-400, 400);
+        
+        // 制御量の最小、最大 
+        RF.setOutputLimits(0x48, 0x7D);
+        RB.setOutputLimits(0x48, 0x7D);
+        LF.setOutputLimits(0x48, 0x7D);
+        LB.setOutputLimits(0x48, 0x7D);
+
+        // よくわからんやつ 
+        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(rpm_RF);
+        RB.setProcessValue(rpm_RB);
+        LF.setProcessValue(rpm_LF);
+        LB.setProcessValue(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;
@@ -228,12 +307,66 @@
 
     while(1)
     {  
+        timer.start();
+        myled = 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);
+        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);
+            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);
+        }
+        timer.reset();
+        */
+        /*
+        RF.reset();
+        RB.reset();
+        LF.reset();
+        LB.reset();
+        //wait(5);
+        */
+        
         
         /*
         // センサ出力値の最小、最大