Revision 4:fad5b5a598bf, committed 2022-02-25

Comitter:

kosukesuzuki

Date:

Fri Feb 25 04:38:59 2022 +0000

Parent:

3:05a780930d12

Commit message:

PID

Changed in this revision

diff -r 05a780930d12 -r fad5b5a598bf main.cpp
--- a/main.cpp	Fri Feb 25 03:15:53 2022 +0000
+++ b/main.cpp	Fri Feb 25 04:38:59 2022 +0000
@@ -36,7 +36,7 @@
 int main() {
     GX = GY = GZ= 0;
     Tgx = Tgy = Tgz = 0;
-    pc.printf("start");
+    pc.printf("start\r\n");
     
      
     motor1 = motor2 = motor3 = motor4 = 0.0;
@@ -70,9 +70,9 @@
     while(1){
         //角度求める
         mpu.readGyroData(gyro);
-        int gx = gyro[0]+3656-3505-50-1850+2223; //変更必須
-        int gy = gyro[1]-30+710-140-300;
-        int gz = gyro[2]+5+195-453+300;
+        int gx = gyro[0]+3656-3505-50-1850+2223-24; //変更必須
+        int gy = gyro[1]-30+710-140-300+375-50;
+        int gz = gyro[2]+5+195-453+300-244-20-10;
         //printf("%d %d %d\r\n",gx,gy,gz);
         
         double gX = gx*0.02562299;//0.025622990.0128114995
@@ -94,7 +94,7 @@
         Tgz = Tgz + abs(gZ2);
         
         
-        if(Tgx > 5){
+        if(Tgx > 3){
             if(GX > 0){
                 GX = GX - 0.3;
                 }else if(GX < 0){
@@ -104,7 +104,7 @@
                         }
                     Tgx=0;
             }        
-        if(Tgy > 5){
+        if(Tgy > 3){
             if(GY > 0){
                 GY = GY - 0.3;
                 }else if(GY < 0){
@@ -114,7 +114,7 @@
                         }
                     Tgy=0;
             }        
-        if(Tgz > 5){
+        if(Tgz > 3){
             if(GZ > 0){
                 GZ = GZ - 0.3;
                 }else if(GZ < 0){
@@ -124,7 +124,7 @@
                         }
                     Tgz=0;
             }
-        //printf("%.2f %.2f %.2f\r\n",GX,GY,GZ);
+        printf("%.2f %.2f %.2f\r\n",GX,GY,GZ);
         
         wait(dt);
         
@@ -136,7 +136,8 @@
 
         c1 = Kp*f1+Ki*f1*dt+Kd*gX1; //トルク
         c2 = Kp*f2+Ki*f2*dt+Kd*gY1;
-        printf("%.2f %.2f",c1,c2);
+        //
+        printf("%.2f\r\n",c1);
         
         //回転トルク→duty比
         double d1,d2;
@@ -149,14 +150,14 @@
         if(d2 > 0.9){
             d2 = 0.9;
             }
-        printf("%.2f",GX);
-        printf("%.2f %.2f\r\n",d1,d2);
+        //printf("%.2f",GX);
+        //printf("%.2f %.2f\r\n",d1,d2);
         
         
         //duty比からmotorを動かす。
         if(GX > 0){
             motor1 = abs(d1);
-            motor2 = 0.5-abs(d1)/2;
+            motor2 = (1-abs(d1))/2;
             }else if(c1 < 0){
                 motor2 = abs(d1);
                 motor1 = 0.5-abs(d1)/2;