Diff: main.cpp

Revision:

3:05a780930d12

Parent:

2:c329b0c4ec92

--- a/main.cpp	Wed Feb 23 12:53:39 2022 +0000
+++ b/main.cpp	Fri Feb 25 03:15:53 2022 +0000
@@ -1,24 +1,32 @@
 //PID制御
 #include "mbed.h"
 #include "MPU6050.h"
+#include "Servo.h"
 
 const double dt = 0.01; //微小時間
 
-const double Kp =0.3; //変更必須
-const double Ki =0.4;
-const double Kd =0.5;
+const double Kp =5; //変更必須
+const double Ki =3;
+const double Kd =3;
 
-const double mc = 77; //最大トルク
+const double mc = 1995; //最大トルク
 
 const double T  =0; //目標角度
 
 MPU6050 mpu(p9,p10);
 Serial pc(USBTX,USBRX);
 
-PwmOut led1(LED1); //motor
+Servo motor1(p21);
+Servo motor2(p22);
+Servo motor3(p23);
+Servo motor4(p24);
+
+/*
+PwmOut led1(LED1); //led
 PwmOut led2(LED2);
 PwmOut led3(LED3);
 PwmOut led4(LED4);
+*/
 
 int gyro[3];
 
@@ -28,16 +36,46 @@
 int main() {
     GX = GY = GZ= 0;
     Tgx = Tgy = Tgz = 0;
+    pc.printf("start");
+    
+     
+    motor1 = motor2 = motor3 = motor4 = 0.0;
+    wait(0.5); 
+    motor1 = motor2 = motor3 = motor4 = 1.0;
+    wait(8);
+    motor1 = motor2 = motor3 = motor4 = 0.0;
+    wait(8);
+    
+    /*
+    //準備
+    for (float p=0.0; p<=0.75; p += 0.25) {
+            motor1 = motor2 = motor3 = motor4 = p;
+            wait(0.5);
+        }
+    
+    wait(1);
+    
+    motor1 = motor2 = motor3 = motor4 = 0;
+    wait(1);
+    
+    motor1 = motor2 = motor3 = motor4 = 1;
+    
+    wait(1);
+    */
+    
+    //準備終了
+    
+    
     
     while(1){
         //角度求める
         mpu.readGyroData(gyro);
-        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);
+        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;
+        //printf("%d %d %d\r\n",gx,gy,gz);
         
-        double gX = gx*0.02562299;
+        double gX = gx*0.02562299;//0.025622990.0128114995
         double gY = gy*0.02562299;
         double gZ = gz*0.02562299;
         int gX1 = gX;
@@ -59,30 +97,36 @@
         if(Tgx > 5){
             if(GX > 0){
                 GX = GX - 0.3;
-                }else{
+                }else if(GX < 0){
                     GX = GX + 0.3;
-                    }
+                    }else{
+                        GX =GX;
+                        }
                     Tgx=0;
             }        
         if(Tgy > 5){
             if(GY > 0){
                 GY = GY - 0.3;
-                }else{
+                }else if(GY < 0){
                     GY = GY + 0.3;
-                    }
+                    }else{
+                        GY = GY;
+                        }
                     Tgy=0;
             }        
         if(Tgz > 5){
             if(GZ > 0){
                 GZ = GZ - 0.3;
-                }else{
+                }else if(GZ < 0){
                     GZ = GZ + 0.3;
-                    }
+                    }else{
+                        GZ = GZ;
+                        }
                     Tgz=0;
             }
         //printf("%.2f %.2f %.2f\r\n",GX,GY,GZ);
         
-        wait(0.01);
+        wait(dt);
         
         //PID()
         double c1,c2;
@@ -91,35 +135,43 @@
         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);
+        c2 = Kp*f2+Ki*f2*dt+Kd*gY1;
+        printf("%.2f %.2f",c1,c2);
         
         //回転トルク→duty比
         double d1,d2;
         d1 = c1/mc;
         d2 = c2/mc;
         
-        if(d1 > 1){
-            d1 = 1;
+        if(d1 > 0.9){
+            d1 = 0.9;
+            }
+        if(d2 > 0.9){
+            d2 = 0.9;
             }
-        if(d2 > 1){
-            d2 = 1;
-            }
-            
-        //duty比からledを動かす。
+        printf("%.2f",GX);
+        printf("%.2f %.2f\r\n",d1,d2);
+        
+        
+        //duty比からmotorを動かす。
         if(GX > 0){
-            led1 = abs(d1);
-            led2 = -abs(d1);
-            }else{
-                led2 = abs(d1);
-                led1 = -abs(d1);
-                }
-        if(GY > 0){
-            led3 = abs(d2);
-            led4 = -abs(d2);
-            }else{
-                led4 = abs(d2);
-                led3 = -abs(d2);
-                }
+            motor1 = abs(d1);
+            motor2 = 0.5-abs(d1)/2;
+            }else if(c1 < 0){
+                motor2 = abs(d1);
+                motor1 = 0.5-abs(d1)/2;
+                }else{
+                    motor1 = motor2 = 0;
+                    }
+                
+        if(GX > 0){
+            motor3 = abs(d2);
+            motor4 = 1-abs(d2);
+            }else if(c2 < 0){
+                motor4 = abs(d2);
+                motor3 = 1-abs(d2);
+                }else{
+                    motor3 = motor4 = 0;
+                    }
         }
     } 
\ No newline at end of file