Diff: main.cpp

Revision:

3:1223cab367d9

Parent:

2:c32991ba628f

Child:

4:df334779a69e

--- a/main.cpp	Sat Aug 18 04:59:28 2018 +0000
+++ b/main.cpp	Mon Aug 27 08:15:36 2018 +0000
@@ -80,30 +80,6 @@
 /* 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();
@@ -140,13 +116,6 @@
     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;
@@ -158,39 +127,15 @@
     /* 速度(秒速)から速度(時速)へ変換 */
     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", 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());
+    pc.printf("前: %d, 後: %d\n", rpm_LB, rpm_LF);
+    //pc.printf("%d\n", rpm_LB);
 
     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_RF);
-    //pc.printf("%lf\n", a_v);
-    //pc.printf("rpm: %d  a_v: %lf\n", rpm[1], a_v);
-}
-
 int front_PID(int setpoint)
 { 
         // センサ出力値の最小、最大 
@@ -211,11 +156,6 @@
         LF.setMode(AUTO_MODE);
         LB.setMode(AUTO_MODE);
         
-        //RF: -
-        //RB: -
-        //LF: +
-        //LB: +
-        
         // 目標値 
         RF.setSetPoint(setpoint);
         RB.setSetPoint(setpoint);
@@ -242,8 +182,7 @@
         
         return 0;
 }
-
-int back_PID(int setpoint)
+int back_PID(int setpoint1, int setpoint2)
 { 
         // センサ出力値の最小、最大 
         RF.setInputLimits(0, 2000);
@@ -264,10 +203,10 @@
         LB.setMode(AUTO_MODE);
         
         // 目標値 
-        RF.setSetPoint(setpoint);
-        RB.setSetPoint(setpoint);
-        LF.setSetPoint(setpoint);
-        LB.setSetPoint(setpoint);
+        RF.setSetPoint(setpoint1);
+        RB.setSetPoint(setpoint1);
+        LF.setSetPoint(setpoint1);
+        LB.setSetPoint(setpoint2);
         
         // センサ出力 
         //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
@@ -284,33 +223,43 @@
        
         return 0;
 }
-
 int main(void)
 {   
     emergency = 0;
     send_data[0] = 0x80;
-    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(0x10, send_data, 1);
+    //i2c.write(0x12, send_data, 1);
+    //i2c.write(0x14, send_data, 1);
+    //i2c.write(0x16, send_data, 1);
+    i2c.write(0x20, send_data, 1);
+    i2c.write(0x22, 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確認
-        back_PID(200);
-        i2c.write(0x20, LF_data, 1, false);
+        front_PID(300);
+        i2c.write(0x20, true_LB_data, 1, false);
+        i2c.write(0x22, true_LF_data, 1, false);
         wait_us(20);
         */
         
+        
+        //back_PID(430, 730);
+        //back_PID(300, 500);
+        back_PID(325, 650);
+        //back_PID(250, 900);
+        //LB_data[0] = 0x0C;
+        //LF_data[0] = 0x0C;
+        i2c.write(0x10, LB_data, 1, false);
+        i2c.write(0x22, LF_data, 1, false);
+        wait_us(20);
+        
+        /*
         timer.start();        
         while(timer.read() <= 5.0f) {
             myled = 1;
@@ -339,49 +288,6 @@
             wait_us(20);
         }
         timer.reset();
-        
-        /*
-        // センサ出力値の最小、最大 
-        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];
         */
         
         /*