Revision 5:e4fe216f01ad, committed 2022-03-04

Comitter:

kosukesuzuki

Date:

Fri Mar 04 07:53:32 2022 +0000

Parent:

4:ec40fbfb90f6

Commit message:

a

Changed in this revision

diff -r ec40fbfb90f6 -r e4fe216f01ad main.cpp
--- a/main.cpp	Mon Feb 28 09:45:43 2022 +0000
+++ b/main.cpp	Fri Mar 04 07:53:32 2022 +0000
@@ -1,7 +1,8 @@
 //｢変更｣と書かれている場所の数値は、自分で決めて変える。（主にセンサー数値の補正）
+//気圧は基盤のミスのため、今回は使わない
 #include "mbed.h"
 #include "MPU6050.h"
-#include "LPS.h"
+//#include "LPS.h"
 #include "SDFileSystem.h"
 #include "Servo.h"
 
@@ -11,16 +12,17 @@
 const double Ki =7; //変更
 const double Kd =5; //変更
 
-const double k =0.25; //プロペラの定数(適当に置いたやつ)
+//const double k =0.25; //プロペラのための定数(今回使わない)
 
 const double mc = 3000; //最大トルク(変更)
 
-const double T  = 0; //目標角度°(変更)
+const double T1 = 90; //目標角度°(変更)
+const double T2 = -90; //目標角度°(変更)
 
 DigitalOut led[]={LED1,LED2,LED3,LED4};
 
-I2C i2c(p28,p27);
-LPS ps(i2c);
+//I2C i2c(p28,p27);
+//LPS ps(i2c);
 MPU6050 mpu(p9,p10);
 SDFileSystem sd(p5, p6, p7, p8, "sd");
 Serial xbee(p13,p14);
@@ -45,6 +47,7 @@
 int main() {
     xbee.printf("motor start\r\n");
     led[0]=1;
+    
     //モーター準備開始(一時待つ事が必要)
     myservo1 = myservo2 = myservo3 = myservo4 = 0.0;
     wait(0.5);
@@ -71,14 +74,16 @@
         count = 0;
         GX = 0,GY = 0,GZ = 0;
         Tgx = 0,Tgy = 0,Tgz = 0;
-    
+        
+        /*
         if (!ps.init()){
             pc.printf("Failed to autodetect pressure sensor!\r\n");
             while (1);
         }
      
         ps.enableDefault();
-    
+        */
+        
         mkdir("/sd/test1", 0777);
         FILE *fp = fopen("/sd/test1/sdtest1.txt", "w");
     
@@ -93,11 +98,11 @@
         
             //角度算出
             mpu.readGyroData(gyro);
-            int gx = gyro[0]+3656-3505-50-1850+2223-24; //変更
-            int gy = gyro[1]-30+710-140-300+375-50-420+255; //変更
-            int gz = gyro[2]+5+195-453+300-244-20-10+95+72; //変更
+            int gx = gyro[0]+420; //変更
+            int gy = gyro[1]-1424+185+26-34+53; //変更
+            int gz = gyro[2]; //変更
             //printf("%d %d %d\r\n",gx,gy,gz);
-         
+            
             double gX = gx*0.02562; //0.0128114995(測定レンジ±500) //変更
             double gY = gy*0.02562; //0.02562299(±250) //変更
             double gZ = gz*0.02562; //変更
@@ -143,7 +148,7 @@
                         }
                     Tgz = 0;
                 }
-             
+            printf("%.2f %.2f %.2f\r\n",GX,GY,GZ);             
         
             //加速度算出
             mpu.readAccelData(accel);
@@ -153,65 +158,90 @@
             float AX = ax*0.000597964111328125; //変更
             float AY = ay*0.000597964111328125; //変更
             float AZ = az*0.000597964111328125; //変更 
-            double ad = AX*AX+AY*AY+AZ*AZ-95.982071137936;
-        
-        
+            double a = AX*AX+AY*AY+AZ*AZ-95.982071137936;
+            
+            /*
             //気圧・標高・温度算出
             float pres = ps.readPressureMillibars();
             float altit = ps.pressureToAltitudeMeters(pres);
             float tempe = ps.readTemperatureC();
-        
+            */
         
             //PID開始
             //必要なトルクを算出
-            double c1,c2;
-            double f1,f2;
-            f1 = T - GX;
-            f2 = T - GY;
-
+            double c1,c2,c3,c4;
+            double f1,f2,f3,f4;
+            
+            //GX
+            f1 = T1 - GX;
+            f2 = T2 - GX;
+            //GY
+            f3 = T1 - GY;
+            f4 = T2 - GY;
+            
+            //GX
             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*gX1;
+            //GY
+            c3 = Kp*f3+Ki*f3*dt-Kd*gY1;
+            c4 = Kp*f4+Ki*f4*dt-Kd*gY1;
+            //printf("%.2f %.2f %.2f %.2f\r\n",c1,c2,c3,c4);　//トルク
         
             //回転トルク→duty比を算出
-            double d1,d2;
+            double d1,d2,d3,d4;
             d1 = c1/mc;
             d2 = c2/mc;
-            
+            d3 = c3/mc;
+            d4 = c4/mc;
             
-            //
-            if(d1 > 0.9){ //変更
-                d1 = 0.9; //変更
+            //出力の限界
+            if(d1 > 0.8){ //変更
+                d1 = 0.8; //変更
+                }
+            if(d2 > 0.8){ //変更
+                d2 = 0.8; //変更
                 }
-            if(d2 > 0.9){ //変更
-                d2 = 0.9; //変更
+            if(d3 > 0.8){ //変更
+                d3 = 0.8; //変更
                 }
-            printf("%.2f %.2f\r\n",d1,d2); //duty比
+            if(d4 > 0.8){ //変更
+                d4 = 0.8; //変更
+                }
+                
+            //printf("%.2f %.2f %.2f %.2f\r\n",d1,d2,d3,d4); //duty比
             
             //試行回数
             count  = count + 1;
             
             //duty比からmotorを動かす。
+            myservo1 = abs(d1);
+            myservo2 = abs(d2);
+            myservo3 = abs(d3);
+            myservo4 = abs(d4);
+            
+            
+            /* duty比が２つだけのとき(定数kを使用)
             if(GX > 0){
             myservo1 = abs(d1);
-            myservo2 = abs(d1)*k;
+            myservo2 = abs(d2);//abs(d1)*k;;
             }else if(GX < 0){
-                myservo2 = abs(d1);
-                myservo1 = abs(d1)*k;
+                myservo2 = abs(d2);
+                myservo1 = ; abs(d1); //abs(d1)*k;; //abs(d1)*k;
                 }else{
-                    myservo1 = myservo2 = 0.025;
+                    myservo1 = myservo2 = ;
                     }
                 
                 if(GY > 0){
-                myservo3 = abs(d2);
-                myservo4 = abs(d2)*k;
+                myservo3 = abs(d2)+0.025;
+                myservo4 = 0.025; //abs(d2)*k; //abs(d2)*k;
                 }else if(GY < 0){
-                myservo4 = abs(d2);
-                myservo3 = abs(d2)*k;
+                myservo4 = abs(d2)+0.025;
+                myservo3 = 0.025; //abs(d2)*k;
                 }else{
                     myservo3 = myservo4 = 0.025;
                     }
-        
+            */
+            
             /*主なセンサー表示
             pc.printf("T %.2f\r\n",ti.read());
             pc.printf("c %d\r\n",count);
@@ -220,14 +250,14 @@
             pc.printf("p%.2f alt%.2f t%.2f\r\n",pres,altit,tempe);
             */
         
-            //pc.printf("c%d, T%.2f, ax%.2f,ay%.2f,az%.2f,|a|%.2f, gx%.2f,gy%.2f,gz%.2f, p%.2f,al%.2f,t%.2f\r\n",count,ti.read(),AX,AY,AZ,a,GX,GY,GZ,pres,altit,tempe);
-            //xbee.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",AX,AY,AZ,a,GX,GY,GZ,pres,altit,tempe);
-            //fprintf(fp, "%d %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\r\n",count,ti.read(),AX,AY,AZ,a,GX,GY,GZ,pres,altit,tempe,c1,c2,d1,d2);
+            pc.printf("c%d, T%.2f, ax%.2f,ay%.2f,az%.2f,|a|%.2f, gx%.2f,gy%.2f,gz%.2f\r\n",count,ti.read(),AX,AY,AZ,a,GX,GY,GZ);
+            xbee.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",AX,AY,AZ,a,GX,GY,GZ);
+            fprintf(fp, "%d %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\r\n",count,ti.read(),AX,AY,AZ,a,GX,GY,GZ,c1,c2,d1,d2);
             //fprintf(fp,"%.2f %.2f %.2f %.2f %.2f\r\n",ti.read(),c1,c2,d1,d2); //調整用
         
             wait(dt);
         
-/*            if(AZ > 9 && AZ < 11 && a > 8 && a < 11 && 5 < ti.read()){
+            if(AZ > 9 && AZ < 11 && a > 8 && a < 11 && 5 < ti.read()){
                 fprintf(fp,"end\r\n");
                 fclose(fp); //注意：ここまで行わないと、SDカードに保存されない
                 xbee.printf("end\r\n");
@@ -236,7 +266,7 @@
                 myservo1 = myservo2 = myservo3 = myservo4 = 0.0;
                 led[2]=1;
                 break;}
-*/              
+              
                 }
             
             //led[1]=0;