Diff: main.cpp

Revision:

27:b4922048ab11

Parent:

23:2ca79873ef44

Child:

28:5e21ce413558

diff -r 2ca79873ef44 -r b4922048ab11 main.cpp
--- a/main.cpp	Fri Dec 07 06:32:14 2018 +0000
+++ b/main.cpp	Tue Dec 11 10:23:35 2018 +0000
@@ -5,6 +5,11 @@
 #include "HMC5883L.h"
 #include "MPU6050.h"
 
+ 
+PwmOut motorSpeed(p26);
+DigitalOut motorDir1(p19);
+DigitalOut motorDir2(p20);
+
 
 Serial pc(USBTX, USBRX);            //地磁気センサー,GPS
 GPS gps(p28, p27);                  //GPS
@@ -34,15 +39,23 @@
 ultrasonic mu(p11, p12, .1, 1, &dist);    //Set the trigger pin to D8 and the echo pin to D9              超音波センサー
                                           //have updates every .1 seconds and a timeout after 1
                                           //second, and call dist when the distance changes
+                                          
+void motorForward(void);
+void motorStop(void);
+void motorReverse(void);                                         
  
 
  
 LocalFileSystem local("local");               // Create the local filesystem under the name "local"　　 データ保存
  
+float culculate_distance_3(float a,float b);
+
 int main() {
     
-   FET = 0; 
-     while(1) {
+    printf("cansat start\r\n");
+    
+  // FET = 0; 
+  /*   while(1) {
     if(flight==1) {
         wait(10);
         }
@@ -59,15 +72,15 @@
         FET = 0; 
     break;
             }
-    }
+    }     */
     
     motor1.stop(0);
     motor2.stop(0);
     
-    wait(30);     //確認用
+    wait(20);     //確認用
     
  printf("GPS start\r\n");
-   FILE *fp = fopen("/local/gps.txt", "w");  // Open "gps.txt" on the local file system for writing
+   FILE *fp = fopen("/local/gps,foto.txt", "w");  // Open "gps.txt" on the local file system for writing
     fprintf(fp, "GPS Start\r\n");
     int n;
     for(n=0;n<45;n++)  //GPSの取得を45回行う(これで大体1分半)
@@ -84,11 +97,8 @@
         printf("%d\r\n",n);              //今何回目かを出力（本番ではいらない）
 
     }    
-    fprintf(fp,"GPS finish\r\n");
+    fprintf(fp,"GPS finish\r\n");    
  //   fclose(fp);                     // GPSの測定終了 
-    
-  
-    
      
 compass.init();                  //地磁気センサー動作
       
@@ -163,7 +173,13 @@
         float oldAccel = 0;//ひとつ前の加速度
         float difference=0;//変位
         float timespan=0.01;//時間差
-        int accel[3];//accelを3つの配列で定義。
+        int accel[3];//accelを3つの配列で定義。*/
+        
+        int tt=0;
+        float run=0;
+        
+         motorSpeed.period_ms(2);              //モーター調節
+         motorSpeed = 0.9;
 
         while(1)
     {       
@@ -200,7 +216,7 @@
         oldSpeed = speed;
 
         //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
-        printf("speed %f diference %f\r\n",speed,difference);//速度（m）と変位を表示
+        printf("speed %f diference %f\r\n",speed,difference);//速度（m）と変位を表示*/
     
         printf("%d\r\n", test.read());              //フォトインタラプタ
         printf("%d\r\n", test2.read());
@@ -225,23 +241,33 @@
         }
         printf("%f", rightrun);
         printf("\t%f\r\n", leftrun2);
-        if (rightrun >= 4396){                             //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
+        run=culculate_distance_3(rightrun,leftrun2);
+        if (run >= 4396){                             //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
             break;                                  //つまりゴールについたらこのループからぬける
         }
         
-         if(difference >= 0.3)
+    /*    if(difference >= 0.3)
          {
              break;
-         }    
+         }    */
         
         motor1.speed(0.5);   //通常走行
-        motor2.speed(0.5);
+        motorForward();
                                //Do something else here
                                // mu.checkDistance(); //call checkDistance() as much as possible, as this is where
                                //the class checks if dist needs to be called.
 
         wait(0.01);
         
+        tt++;
+        if(tt==10)
+        {
+            fprintf(fp, "accel.rightrun.leftrun2\r\n");
+            
+            fprintf(fp,"(%lf, %lf,%lf)\r\n", difference, rightrun, leftrun2);//加速度とフォトインタラプタによる距離を出力
+            tt=0;
+        }    
+        
         if(100 < distance && distance < 500)        //障害物発見❕
          {
             
@@ -260,7 +286,7 @@
             printf("mortor stop\r\n");
 
             motor1.speed(msj1);       //機体を時計回りに９０度回転
-            motor2.speed(-msj2);
+            motorForward();
             wait(0.77);
             printf("mortor rotation\r\n");
             
@@ -270,7 +296,7 @@
             printf("mortor stop\r\n");
 
             motor1.speed(ms1);       //直進
-            motor2.speed(ms2);
+            motorForward();
             wait(2);
             printf("mortor straight\r\n");
             
@@ -280,7 +306,7 @@
             printf("mortor stop\r\n");
 
             motor1.speed(-msj1);      //機体を反時計回りに９０度回転
-            motor2.speed(msj2);
+            motorForward();
             wait(0.77);
             printf("mortor rotation\r\n");
             
@@ -290,11 +316,11 @@
             printf("mortor stop\r\n");
 
             motor1.speed(ms1);       //直進
-            motor2.speed(ms2);
+            motorForward();
             
             
         int t=0;
-    for(t=0;t<100;t++)
+    for(t=0;t<50;t++)
     {
             printf("%d\r\n", test.read());
             printf("%d\r\n", test2.read());                   
@@ -349,9 +375,14 @@
         oldSpeed = speed;
 
         //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
-        printf("speed %f diference %f\r\n",speed,difference);//速度（m）と変位を表示
+        printf("speed %f diference %f\r\n",speed,difference);//速度（m）と変位を表示*/
+        
+        if (run >= 4396){                             //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
+            break;                                  //つまりゴールについたらこのループからぬける
+        }
         
         wait(0.01);
+           
     }      
     
     
@@ -364,7 +395,7 @@
             printf("mortor stop\r\n");         
 
             motor1.speed(-msj1);      //機体を反時計回りに９０度回転
-            motor2.speed(msj2);
+            motorForward();
             wait(1);
             printf("mortor rotation\r\n");
             
@@ -374,7 +405,7 @@
             printf("mortor stop\r\n");
 
             motor1.speed(ms1);       //直進
-            motor2.speed(ms2);
+            motorForward();
             wait(2);
             printf("mortor straight\r\n");
             
@@ -384,7 +415,7 @@
             printf("mortor stop\r\n");
 
             motor1.speed(msj1);       //機体を時計回りに９０度回転
-            motor2.speed(-msj2);
+            motorForward();
             wait(1);   
             printf("mortor rotation\r\n");
             
@@ -401,7 +432,9 @@
             
             
             fprintf(fp, "last accel.photo\r\n");
-            fprintf(fp,"(%lf, %lf)\r\n", difference, rightrun);//最後の加速度とフォトインタラプタによる距離を出力
+            fprintf(fp,"(%lf, %lf)\r\n", difference, run);//最後の加速度とフォトインタラプタによる距離を出力
+            fprintf(fp, "last right.left\r\n");
+            fprintf(fp,"(%lf, %lf)\r\n", rightrun, leftrun2);
             fclose(fp);
     
 }
@@ -414,8 +447,23 @@
                c=0.5*a+0.5*b;//今は平均。計測をもとに修正を加える
                return c;
             }
+void motorForward() {
+    motorStop();
+    motorDir1 = 1;
+    motorDir2 = 0;
+}
+ 
+void motorReverse() {
+    motorStop();
+    motorDir1 = 0;
+    motorDir2 = 1;
+}
+ 
+void motorStop() {
+    motorDir1 = 0;
+    motorDir2 = 0;
+}
 
 
 
 
-