lpc1768 aassss

Dependencies:   mbed Servo LPS25HB_I2C MPU6050 SDFileSystem

Files at this revision

API Documentation at this revision

Comitter:
kosukesuzuki
Date:
Mon Feb 28 09:45:43 2022 +0000
Parent:
3:14b178724982
Commit message:
a

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 14b178724982 -r ec40fbfb90f6 main.cpp
--- a/main.cpp	Fri Feb 25 06:06:57 2022 +0000
+++ b/main.cpp	Mon Feb 28 09:45:43 2022 +0000
@@ -7,13 +7,17 @@
 
 const double dt = 0.01; //微小時間(変更)
 
-const double Kp =5; //変更
-const double Ki =3; //変更
-const double Kd =3; //変更
+const double Kp =11; //変更
+const double Ki =7; //変更
+const double Kd =5; //変更
+
+const double k =0.25; //プロペラの定数(適当に置いたやつ)
 
-const double mc = 1995; //最大トルク(変更)
+const double mc = 3000; //最大トルク(変更)
 
-const double T  =0; //目標角度°(変更)
+const double T  = 0; //目標角度°(変更)
+
+DigitalOut led[]={LED1,LED2,LED3,LED4};
 
 I2C i2c(p28,p27);
 LPS ps(i2c);
@@ -22,15 +26,13 @@
 Serial xbee(p13,p14);
 Serial pc(USBTX,USBRX);
 
-Servo motor1(p21);
-Servo motor2(p22);
-Servo motor3(p23);
-Servo motor4(p24);
+Servo myservo1(p21);
+Servo myservo2(p22);
+Servo myservo3(p23);
+Servo myservo4(p24);
 
 Timer ti;
 
-int cmd = xbee.getc();
-
 int gyro[3];
 int accel[3];
 
@@ -38,21 +40,31 @@
 double GX,GY,GZ;
 double Tgx,Tgy,Tgz;
 
-
+int cmd;
 
 int main() {
+    xbee.printf("motor start\r\n");
+    led[0]=1;
     //モーター準備開始(一時待つ事が必要)
-    motor1 = motor2 = motor3 = motor4 = 0.0;
-    wait(0.5); 
-    motor1 = motor2 = motor3 = motor4 = 1.0;
+    myservo1 = myservo2 = myservo3 = myservo4 = 0.0;
+    wait(0.5);
+    myservo1 = myservo2 = myservo3 = myservo4 = 1.0;
     wait(8);
-    motor1 = motor2 = motor3 = motor4 = 0.0;
+    myservo1 = myservo2 = myservo3 = myservo4 = 0.0;
     wait(8);
+    xbee.printf("motor end\r\n");
     //モーター準備終了
     
-    //pcのxbeeからのコマンド「c」で開始
-    if(cmd = 'c'){
+    led[1]=1;
+    //約5秒で開始
+    wait(5);
     
+    wait(1.0);
+    led[0]=1;
+    wait(1.0);
+    led[0]=0;
+
+    led[1]=0;
         xbee.printf("start\r\n");
         pc.printf("start\r\n");
     
@@ -74,7 +86,6 @@
     
         ti.start();
         while(1){
-         
             if(fp == NULL) {
             error("Could not open file for write\n");
             }
@@ -83,8 +94,8 @@
             //角度算出
             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; //変更
+            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; //変更
             //printf("%d %d %d\r\n",gx,gy,gz);
          
             double gX = gx*0.02562; //0.0128114995(測定レンジ±500) //変更
@@ -142,7 +153,7 @@
             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;
+            double ad = AX*AX+AY*AY+AZ*AZ-95.982071137936;
         
         
             //気圧・標高・温度算出
@@ -158,47 +169,48 @@
             f1 = T - GX;
             f2 = T - GY;
 
-            c1 = Kp*f1+Ki*f1*dt+Kd*gX1; //トルク
-            c2 = Kp*f2+Ki*f2*dt+Kd*gY1;
+            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比
-        
-        
+            printf("%.2f %.2f\r\n",d1,d2); //duty比
+            
             //試行回数
             count  = count + 1;
-        
+            
             //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;
-                        }
+            myservo1 = abs(d1);
+            myservo2 = abs(d1)*k;
+            }else if(GX < 0){
+                myservo2 = abs(d1);
+                myservo1 = abs(d1)*k;
+                }else{
+                    myservo1 = myservo2 = 0.025;
+                    }
                 
-            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;
-                        }
+                if(GY > 0){
+                myservo3 = abs(d2);
+                myservo4 = abs(d2)*k;
+                }else if(GY < 0){
+                myservo4 = abs(d2);
+                myservo3 = abs(d2)*k;
+                }else{
+                    myservo3 = myservo4 = 0.025;
+                    }
         
             /*主なセンサー表示
             pc.printf("T %.2f\r\n",ti.read());
@@ -208,22 +220,26 @@
             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, 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()){
+/*            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");
                 pc.printf("end\r\n");
                 ti.stop();
-                return 0;
+                myservo1 = myservo2 = myservo3 = myservo4 = 0.0;
+                led[2]=1;
+                break;}
+*/              
                 }
             
-            }
-        }
-}
\ No newline at end of file
+            //led[1]=0;
+            //led[3]=1;
+            
+        }
\ No newline at end of file