Revision 3:14b178724982, committed 2022-02-25

Comitter:

kosukesuzuki

Date:

Fri Feb 25 06:06:57 2022 +0000

Parent:

2:194e00108f75

Commit message:

hh

Changed in this revision

diff -r 194e00108f75 -r 14b178724982 Servo.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Fri Feb 25 06:06:57 2022 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Servo/#36b69a7ced07

diff -r 194e00108f75 -r 14b178724982 main.cpp
--- a/main.cpp	Thu Feb 17 15:08:56 2022 +0000
+++ b/main.cpp	Fri Feb 25 06:06:57 2022 +0000
@@ -1,7 +1,19 @@
+//｢変更｣と書かれている場所の数値は、自分で決めて変える。（主にセンサー数値の補正）
 #include "mbed.h"
 #include "MPU6050.h"
 #include "LPS.h"
 #include "SDFileSystem.h"
+#include "Servo.h"
+
+const double dt = 0.01; //微小時間(変更)
+
+const double Kp =5; //変更
+const double Ki =3; //変更
+const double Kd =3; //変更
+
+const double mc = 1995; //最大トルク(変更)
+
+const double T  =0; //目標角度°(変更)
 
 I2C i2c(p28,p27);
 LPS ps(i2c);
@@ -10,114 +22,208 @@
 Serial xbee(p13,p14);
 Serial pc(USBTX,USBRX);
 
+Servo motor1(p21);
+Servo motor2(p22);
+Servo motor3(p23);
+Servo motor4(p24);
+
+Timer ti;
+
+int cmd = xbee.getc();
+
 int gyro[3];
 int accel[3];
 
+int count;
 double GX,GY,GZ;
 double Tgx,Tgy,Tgz;
 
+
+
 int main() {
-    xbee.printf("start\r\n");
-    pc.printf("start\r\n");
+    //モーター準備開始(一時待つ事が必要)
+    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);
+    //モーター準備終了
     
-    GX = 0,GY = 0,GZ = 0;
-    Tgx = 0,Tgy = 0,Tgz = 0;
+    //pcのxbeeからのコマンド｢c｣で開始
+    if(cmd = 'c'){
     
-    if (!ps.init()){
-        pc.printf("Failed to autodetect pressure sensor!\r\n");
-        while (1);
-    }
+        xbee.printf("start\r\n");
+        pc.printf("start\r\n");
     
-    ps.enableDefault();
+        count = 0;
+        GX = 0,GY = 0,GZ = 0;
+        Tgx = 0,Tgy = 0,Tgz = 0;
     
-    mkdir("/sd/test1", 0777);
-    FILE *fp = fopen("/sd/test1/sdtest1.txt", "w");
+        if (!ps.init()){
+            pc.printf("Failed to autodetect pressure sensor!\r\n");
+            while (1);
+        }
+     
+        ps.enableDefault();
     
-    fprintf(fp,"start\r\n");
+        mkdir("/sd/test1", 0777);
+        FILE *fp = fopen("/sd/test1/sdtest1.txt", "w");
     
-    for(int k = 0; k < 50; k++){
+        fprintf(fp,"start\r\n");
+    
+        ti.start();
+        while(1){
          
-         if(fp == NULL) {
-        error("Could not open file for write\n");
-        }
+            if(fp == NULL) {
+            error("Could not open file for write\n");
+            }
         
-         mpu.readGyroData(gyro);
-         int gx = gyro[0]+146;
-         int gy = gyro[1]+232;  //まだ
-         int gz = gyro[2]+7;
-         //printf("%d %d %d\r\n",gx,gy,gz);
+        
+            //角度算出
+            mpu.readGyroData(gyro);
+            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.02562; //0.0128114995(測定レンジ±500)
-         double gY = gy*0.02562; //0.02562299(±250)
-         double gZ = gz*0.02562;
+            double gX = gx*0.02562; //0.0128114995(測定レンジ±500) //変更
+            double gY = gy*0.02562; //0.02562299(±250) //変更
+            double gZ = gz*0.02562; //変更
          
-         int gX1 = gX;
-         int gY1 = gY;
-         int gZ1 = gZ;
+            int gX1 = gX;
+            int gY1 = gY;
+            int gZ1 = gZ;
          
-         double gX2 = gX1*0.01;
-         double gY2 = gY1*0.01;
-         double gZ2 = gZ1*0.01;
+            double gX2 = gX1*dt;
+            double gY2 = gY1*dt;
+            double gZ2 = gZ1*dt;
          
-         GX = GX + gX2;
-         GY = GY + gY2;
-         GZ = GZ + gZ2;
+            GX = GX + gX2;
+            GY = GY + gY2;
+            GZ = GZ + gZ2;
          
-         Tgx = Tgx + abs(gX2);
-         Tgy = Tgy + abs(gY2);
-         Tgz = Tgz + abs(gZ2);
+            Tgx = Tgx + abs(gX2);
+            Tgy = Tgy + abs(gY2);
+            Tgz = Tgz + abs(gZ2);
          
-         if(Tgx > 10){
-             if(GX > 0){
-                GX = GX - 0.3;//変更必須
-                }else{
-                    GX = GX + 0.3;
+            //和の補正(精度はよくない)
+            if(Tgx > 3){
+                if(GX > 0){
+                    GX = GX - 0.3;//変更
+                    }else{
+                        GX = GX + 0.3;//変更
+                    }
+                Tgx = 0;
                 }
-             Tgx = 0;
-             }
-         if(Tgy > 10){
-             if(GY >0){
-                 GY = GY - 0.3;
-                }else{
-                    GY = GY + 0.3;
-                } 
-             Tgy = 0;
-             }
-         if(Tgz > 10){
-             if(GZ >0){
-                 GZ = GZ - 0.3;
-                 }else{
-                     GZ = GZ + 0.3;
-                     }
-             Tgz = 0;
-             }
+            if(Tgy > 3){
+                if(GY >0){
+                    GY = GY - 0.3;//変更
+                    }else{
+                        GY = GY + 0.3;//変更
+                    } 
+                Tgy = 0;
+                }
+            if(Tgz > 3){
+                if(GZ >0){
+                    GZ = GZ - 0.3;//変更
+                    }else{
+                        GZ = GZ + 0.3;//変更
+                        }
+                    Tgz = 0;
+                }
              
-        mpu.readAccelData(accel);
-        int ax = accel[0]-123;//x軸方向の加速度
-        int ay = accel[1]+60;//y軸方向の加速度
-        int az = accel[2]+1110 ;//z軸方向の加速度
-        float AX = ax*0.000597964111328125;
-        float AY = ay*0.000597964111328125;
-        float AZ = az*0.000597964111328125; 
-        double a = AX*AX+AY*AY+AZ*AZ-95.982071137936;
+        
+            //加速度算出
+            mpu.readAccelData(accel);
+            int ax = accel[0]-123; //変更
+            int ay = accel[1]+60; //変更
+            int az = accel[2]+1110 ; //変更
+            float AX = ax*0.000597964111328125; //変更
+            float AY = ay*0.000597964111328125; //変更
+            float AZ = az*0.000597964111328125; //変更 
+            double a = AX*AX+AY*AY+AZ*AZ-95.982071137936;
+        
+        
+            //気圧・標高・温度算出
+            float pres = ps.readPressureMillibars();
+            float altit = ps.pressureToAltitudeMeters(pres);
+            float tempe = ps.readTemperatureC();
+        
         
-        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;
+
+            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,d2;
+            d1 = c1/mc;
+            d2 = c2/mc;
+        
+            if(d1 > 0.9){ //変更
+                d1 = 0.9; //変更
+                }
+            if(d2 > 0.9){ //変更
+                d2 = 0.9; //変更
+                }
+            //printf("%.2f %.2f\r\n",d1,d2);　//duty比
+        
+        
+            //試行回数
+            count  = count + 1;
         
-        pc.printf("%.2f %.2f %.2f %.2f\r\n",AX,AY,AZ,a);
-        pc.printf("%.2f %.2f %.2f\r\n",GX,GY,GZ);
-        pc.printf("%.2f %.2f %.2f\r\n",pres,altit,tempe);
+            //duty比からmotorを動かす。
+            if(GX > 0){
+                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 = 0.5-abs(d2)/2; //変更
+                }else if(c2 < 0){
+                    motor4 = abs(d2);
+                    motor3 = 0.5-abs(d2)/2; //変更
+                    }else{
+                        motor3 = motor4 = 0;
+                        }
         
-        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, "%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",AX,AY,AZ,a,GX,GY,GZ,pres,altit,tempe);
-        wait(0.01);
+            /*主なセンサー表示
+            pc.printf("T %.2f\r\n",ti.read());
+            pc.printf("c %d\r\n",count);
+            pc.printf("ax%.2f ay%.2f az%.2f |a|%.2f\r\n",AX,AY,AZ,a);
+            pc.printf("gx%.2f gy%.2f gz%.2f\r\n",GX,GY,GZ);
+            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);
+            //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 && 3 < ti.read()){
+                fprintf(fp,"end\r\n");
+                fclose(fp); //注意：ここまで行わないと、SDカードに保存されない
+                xbee.printf("end\r\n");
+                pc.printf("end\r\n");
+                ti.stop();
+                return 0;
+                }
+            
+            }
         }
-        
-        fprintf(fp,"end\r\n");
-        fclose(fp);
-        xbee.printf("end\r\n");
-        pc.printf("end\r\n");
-        
-        return 0;
 }
\ No newline at end of file