Diff: main.cpp

Revision:

2:4a512a6e6b1d

Parent:

0:d6107da1cba0

Child:

3:94b960cbb1bd

--- a/main.cpp	Fri May 07 03:29:23 2021 +0000
+++ b/main.cpp	Thu May 13 05:12:39 2021 +0000
@@ -1,18 +1,133 @@
 #include "mbed.h"
+#include <stdlib.h>
 #include "MPU6050.h"
 #include "QEI.h"
 #include "TB6612FNG.h"
 
-MPU6050 mpu1(p9,p10,0x68);
-MPU6050 mpu2(p9,p10,0x69);
+#define MPU6050_WHO_AM_I     0x75  // Read Only
+#define MPU6050_PWR_MGMT_1   0x6B  // Read and Write
+#define MPU_ADDRESS  0x68
+#define MPU_ADDRESS2  0x69
+#define TYP1 16384.0 //
+#define TYP2 8192.0 //
+#define TYP3 4096.0 //
+#define TYP4 2048.0 //
+#define period 100000
+#define rate 32
+ int t_1, t_2;
+int i = -1;
+int j;
+bool flug = false;
+unsigned long tm, tm2, tm3 = 0;
+int ax,ay,az;
+//float ax2,ay2,az2;
+float Role = 0;
+int num;
+char ch;
+float w = 0; 
+float dw = 0.1;
+
+//初期設定
+//加速度センサ
+//MPU6050 mpu1(p9,p10,0x68);
+//MPU6050 mpu2(p9,p10,0x69);
+//シリアル
 Serial pc(USBTX,USBRX);
-QEI wheel(p22, p21, NC, 12, QEI::X4_ENCODING);
+//エンコーダ
+QEI wheel(p22, p21, NC, 6, QEI::X2_ENCODING);
+//タイマー
+Ticker flipper;
+void flip() {
+    flug = true;
+    //pc.printf("AA");
+}
+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();
+    //mpu1.start();
+    //mpu2.start();
+    motor.STBY(1);
+    motor.Forward();
+    //pc.printf("Hello3\n");
     while(1){
-        pc.printf("%.2f\n", ((float)wheel.getPulses()));
-        pc.printf("%.2f\n", ((float)wheel.getRevolutions()));
-        wait(0.1);
+        if(i == -1){
+            if(pc.readable()) {
+                char ch[5];    // 受信確認
+                while(pc.readable()){
+                    if(j == 0)wait(0.1);
+                    //pc.printf("%d\n", j);
+                    ch[j] = pc.getc();
+                    j++;
+                    //wait(0.01);
+                }    // 1文字取り出し
+                ch[j] = '\0';
+                num = atoi(ch);
+                j = 0;
+                switch(num){
+                    case -1:
+                        i = 0;
+                        flipper.attach_us(&flip, period);
+                        break;
+                    case -2:
+                        motor.Stop();
+                        break;
+                    case -3:
+                        motor.Forward();
+                        break;
+                    case -4:
+                        break;
+                    default: 
+                        motor.SetW(float(num)/255);
+                        break;
+                }  
+            }
+        }
+              //      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;
+            }
+        }
     }
-}
\ No newline at end of file
+}