Diff: main.cpp

Revision:

1:036305ced8fd

Parent:

0:1ce8dacdff38

--- a/main.cpp	Wed Feb 23 06:16:49 2022 +0000
+++ b/main.cpp	Wed Feb 23 12:40:26 2022 +0000
@@ -2,36 +2,59 @@
 #include "mbed.h"
 #include "MPU6050.h"
 
-const double Kp =0.3;
+const double dt = 0.01; //微小時間
+
+const double Kp =0.3; //変更必須
 const double Ki =0.4;
 const double Kd =0.5;
 
-const double T  =0; //目標
+const double mc = 77; //最大トルク
+
+const double T  =0; //目標角度
 
 MPU6050 mpu(p9,p10);
 Serial pc(USBTX,USBRX);
-PwmOut led(LED1);
+
+PwmOut led1(LED1); //motor
+PwmOut led2(LED2);
+PwmOut led3(LED3);
+PwmOut led4(LED4);
 
 int gyro[3];
 
-double GX;
-double Tgx;
+double GX,GY,GZ;
+double Tgx,Tgy,Tgz;
 
 int main() {
-    GX = 0;
+    GX = GY = GZ= 0;
+    Tgx = Tgy = Tgz = 0;
+    
     while(1){
         //角度求める
         mpu.readGyroData(gyro);
-        int gx = gyro[0]+3656-3505;
-        //printf("%d\r\n",gx);
+        int gx = gyro[0]+3656-3505-50; //変更必須
+        int gy = gyro[1]-30;
+        int gz = gyro[2]+5;
+        printf("%d %d %d\r\n",gx,gy,gz);
         
         double gX = gx*0.02562299;
+        double gY = gy*0.02562299;
+        double gZ = gz*0.02562299;
         int gX1 = gX;
-        double gX2 = gX1*0.01;
+        int gY1 = gY;
+        int gZ1 = gZ;
+        double gX2 = gX1*dt;
+        double gY2 = gY1*dt;
+        double gZ2 = gZ1*dt;
         
         GX = GX + gX2;
+        GY = GY + gY2;
+        GZ = GZ + gZ2;
         
         Tgx = Tgx + abs(gX2);
+        Tgy = Tgy + abs(gY2);
+        Tgz = Tgz + abs(gZ2);
+        
         
         if(Tgx > 5){
             if(GX > 0){
@@ -40,28 +63,59 @@
                     GX = GX + 0.3;
                     }
                     Tgx=0;
+            }        
+        if(Tgy > 5){
+            if(GY > 0){
+                GY = GY - 0.3;
+                }else{
+                    GY = GY + 0.3;
+                    }
+                    Tgy=0;
+            }        
+        if(Tgz > 5){
+            if(GZ > 0){
+                GZ = GZ - 0.3;
+                }else{
+                    GZ = GZ + 0.3;
+                    }
+                    Tgz=0;
             }
-        //printf("GX %.2f\r\n",GX);
+        //printf("%.2f %.2f %.2f\r\n",GX,GY,GZ);
         
         wait(0.01);
+        
         //PID()
-        double c;
-        double f1;
+        double c1,c2;
+        double f1,f2;
         f1 = T - GX;
-        c = Kp*f1+Ki*f1*0.01+Kd*gX1;
-        //printf("c %.2f\r\n",c);
+        f2 = T - GY;
+
+        c1 = Kp*f1+Ki*f1*dt+Kd*gX1; //トルク
+        c2 = Kp+f2+Ki*f2*dt+Kd*gY1;
+        //printf("%.2f %.2f\r\n",c1,c2);
         
         //回転トルク→duty比
-        
-        double d1;
-        d1 = abs(c)/77;
+        double d1,d2;
+        d1 = c1/mc;
+        d2 = c2/mc;
         
         if(d1 > 1){
             d1 = 1;
             }
-       
+        if(d2 > 1){
+            d2 = 1;
+            }
+            
         //duty比からledを動かす。
-        
-        led = d1;
+        if(GX > 0){
+            led1 = abs(d1);
+            }else{
+                led2 = abs(d1);
+                }
+        if(GY > 0){
+            led3 = abs(d2);
+            }else{
+                led4 = abs(d2);
+                }
         }
     } 
\ No newline at end of file