Diff: main.cpp

Revision:

3:94b960cbb1bd

Parent:

2:4a512a6e6b1d

Child:

4:341ad97bf8a6

--- a/main.cpp	Thu May 13 05:12:39 2021 +0000
+++ b/main.cpp	Fri May 14 00:48:28 2021 +0000
@@ -14,56 +14,95 @@
 #define TYP4 2048.0 //
 #define period 100000
 #define rate 32
- int t_1, t_2;
-int i = -1;
+int t_1, t_2, t_3, t_4;
+int t_21, t_22, t_23;
+int t_11, t_12, t_13;
+int i = rate;
 int j;
-bool flug = false;
+bool flug = true;
 unsigned long tm, tm2, tm3 = 0;
-int ax,ay,az;
+int16_t ax,ay,az;
 //float ax2,ay2,az2;
 float Role = 0;
 int num;
 char ch;
 float w = 0; 
 float dw = 0.1;
-
+Timer t;
 //初期設定
 //加速度センサ
-//MPU6050 mpu1(p9,p10,0x68);
-//MPU6050 mpu2(p9,p10,0x69);
+MPU6050 mpu1(p9,p10,0x68);
+MPU6050 mpu2(p9,p10,0x69);
+Serial pc(USBTX,USBRX);
+//pc.baud(115200);
+//pc.baud(9600);
+//pc.baud(230400);
+//pc.baud(200000);
+char data2[6];
+char data3[6];
 //シリアル
-Serial pc(USBTX,USBRX);
+//Serial pc(USBTX,USBRX);
 //エンコーダ
 QEI wheel(p22, p21, NC, 6, QEI::X2_ENCODING);
 //タイマー
 Ticker flipper;
 void flip() {
-    flug = true;
-    //pc.printf("AA");
+        t_1 = t.read_us();
+        pc.printf("%d\n", t_1 - t_2);
+        if (mpu1.read_data(mpu1.ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6)) {
+            pc.printf("%d\n", int(data2[0] << 8 | data2[1]));
+            t_11 = t.read_us();
+            pc.printf("%d\n", t_11 - t_2);
+            pc.printf("%d\n", int(data2[2] << 8 | data2[3]));
+            t_12 = t.read_us();
+            pc.printf("%d\n", t_12 - t_2);
+            pc.printf("%d\n", int(data2[4] << 8 | data2[5]));
+        }
+        //pc.printf("0\n1\n2\n");
+        t_3 = t.read_us();
+        pc.printf("%d\n", t_3 - t_2);
+        if (mpu2.read_data(mpu2.ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6)) {
+            pc.printf("%d\n", int(data2[0] << 8 | data2[1]));
+            t_21 = t.read_us();
+            pc.printf("%d\n", t_21 - t_2);
+            pc.printf("%d\n", int(data2[2] << 8 | data2[3]));
+            t_22 = t.read_us();
+            pc.printf("%d\n", t_22 - t_2);
+            pc.printf("%d\n", int(data2[4] << 8 | data2[5]));
+        }
+        //pc.printf("%d\n%d\n%d\n",ax,ay,az);
+        //pc.printf("0\n1\n2\n");
+        t_4 = t.read_us();
+        pc.printf("%d\n", t_4 - t_2);
+        t_2 = t_1;
+        i--;
+        if(!i){
+            flipper.detach();
+            t_1 = t.read_us();
+            Role = ((float)wheel.getPulses())*1000000 / (6*t_1);
+            pc.printf("%f\n", Role);
+            flug = true;
+            i = rate;
+        }
 }
-Timer t;
-//モータ
 
 
 int main(){
     TB6612FNG motor(p18, p19, p20, p26);
-    ax = 0;
-    ay = 1;
-    az = 3;
-    Serial pc(USBTX,USBRX);
-    pc.baud(115200);
-    //pc.baud(9600);
-    //pc.baud(230400);
-    //pc.baud(200000);
-    //mpu1.start();
-    //mpu2.start();
+    //pc.baud(115200);
+//pc.baud(9600);
+    pc.baud(230400);
+//pc.baud(200000);
+
+    mpu1.start();
+    mpu2.start();
     //mpu1.start();
     //mpu2.start();
     motor.STBY(1);
     motor.Forward();
     //pc.printf("Hello3\n");
     while(1){
-        if(i == -1){
+        if(flug){
             if(pc.readable()) {
                 char ch[5];    // 受信確認
                 while(pc.readable()){
@@ -78,8 +117,12 @@
                 j = 0;
                 switch(num){
                     case -1:
-                        i = 0;
-                        flipper.attach_us(&flip, period);
+                        flug = false;
+                        t_2 = 0;
+                        t.reset();
+                        wheel.reset();
+                        t.start();
+                        flipper.attach(&flip, 0.1);
                         break;
                     case -2:
                         motor.Stop();
@@ -95,39 +138,5 @@
                 }  
             }
         }
-              //      t.reset();
-              //      t.start();
-        if(flug){
-            //tm = t.read_us();
-            //tm2 = tm - tm3;
-            //tm3 = tm;
-            flug = false;
-            if(!i){
-                t_2 = 0;
-                t.reset();
-                wheel.reset();
-                t.start();
-            }
-            //pc.printf("Hello\n");
-            //mpu1.read(&gx2,&gy2,&gz2,&ax2,&ay2,&az2);
-            //pc.printf("%f\n%f\n%f\n",ax2,ay2,az2);
-            //mpu1.readraw2(&ax,&ay,&az);
-            pc.printf("%d\n%d\n%d\n",ax,ay,az);
-            //mpu2.read(&gx2,&gy2,&gz2,&ax2,&ay2,&az2);
-            //pc.printf("%f\n%f\n%f\n",ax2,ay2,az2);
-            //mpu2.readraw2(&ax,&ay,&az);
-            pc.printf("%d\n%d\n%d\n",ax,ay,az);
-            t_1 = t.read_us();
-            pc.printf("%d\n", t_1 - t_2);
-            t_2 = t_1;
-            i++;
-            if(i >= rate){
-                flipper.detach();
-                t_1 = t.read_us();
-                Role = ((float)wheel.getPulses())*1000000 / (6*t_1);
-                pc.printf("%f\n", Role);
-                i = -1;
-            }
-        }
     }
 }