Diff: main.cpp

Revision:

0:1ce8dacdff38

Child:

1:036305ced8fd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Feb 23 06:16:49 2022 +0000
@@ -0,0 +1,67 @@
+//PID制御
+#include "mbed.h"
+#include "MPU6050.h"
+
+const double Kp =0.3;
+const double Ki =0.4;
+const double Kd =0.5;
+
+const double T  =0; //目標
+
+MPU6050 mpu(p9,p10);
+Serial pc(USBTX,USBRX);
+PwmOut led(LED1);
+
+int gyro[3];
+
+double GX;
+double Tgx;
+
+int main() {
+    GX = 0;
+    while(1){
+        //角度求める
+        mpu.readGyroData(gyro);
+        int gx = gyro[0]+3656-3505;
+        //printf("%d\r\n",gx);
+        
+        double gX = gx*0.02562299;
+        int gX1 = gX;
+        double gX2 = gX1*0.01;
+        
+        GX = GX + gX2;
+        
+        Tgx = Tgx + abs(gX2);
+        
+        if(Tgx > 5){
+            if(GX > 0){
+                GX = GX - 0.3;
+                }else{
+                    GX = GX + 0.3;
+                    }
+                    Tgx=0;
+            }
+        //printf("GX %.2f\r\n",GX);
+        
+        wait(0.01);
+        //PID()
+        double c;
+        double f1;
+        f1 = T - GX;
+        c = Kp*f1+Ki*f1*0.01+Kd*gX1;
+        //printf("c %.2f\r\n",c);
+        
+        //回転トルク→duty比
+        
+        double d1;
+        d1 = abs(c)/77;
+        
+        if(d1 > 1){
+            d1 = 1;
+            }
+       
+        //duty比からledを動かす。
+        
+        led = d1;
+        }
+    } 
\ No newline at end of file