Diff: main.cpp

Revision:

3:8d904225adfd

Parent:

2:6f9881435f17

Child:

4:1fdffa2e6312

--- a/main.cpp	Sun Feb 26 16:46:37 2017 +0000
+++ b/main.cpp	Tue Feb 28 15:54:53 2017 +0000
@@ -54,8 +54,8 @@
 float count_offset,count_gps;
 int a,b,gp,gga,rmc,gll,flag,count2,count_move,quadrant;
 float x_target,y_target,d_target,sita,angle_caly,angle_calx,direction_target;
-float longitude_target = 36.065726;//36.11707
-float latitude_target = 139.272254;//139.4720
+float longitude_target = 36.115986;//36.11707
+float latitude_target = 139.456207;//139.4720
 float utc_time,latitude1,latitude2,longitude1,longitude2,speed,course,utc_date,gps_magnetic;
 char pos_status;
 float direction,x_dat,y_dat,longitudegosa,latitudegosa;
@@ -99,7 +99,7 @@
     
     bit4=1;
     pc.printf("gps_read\n");
-    for(count2=0; count2 <= 250 ; count2++) {
+    for(count2=0; count2 < 200 ; count2++) {
         gps_s[count2] = gps.getc();
         pc.printf("%c",gps_s[count2]);
     }
@@ -111,36 +111,39 @@
 int gps_save()
 {
     a=0;
-    gp = 2;
+    gp = 2;//1回目の＄は無視（flug=1を無視）
     count_gps = 0;
 
     pc.printf("\ngps_save\n");
 
-    while(a <= 250 && count_gps < 5) {
+    while(a <= 250 && count_gps < 4) {
 
-        for(a=0,b=0,flag=0;flag < gp && flag < 5 && gp < 5 && a < 200; a++,b++) {
-            
+        for(a=0,b=0,flag=0;  flag <= gp && flag < 4 && gp < 4 && a < 200;  a++,b++)//"lag<4" "gp<4" "a<200"は無限ループ対策
+         {
             bit3=1;
             gps_c[a] = gps_s[b];
             pc.printf("%d %c \n",a,gps_c[a]);
 
-            if(gps_c[a] == '$') {
+            if(gps_c[a] == '$')
+              {
                 gps_c[0] = '$';
                 a=0;
-                flag++;
+                flag++;//1回目の＄は無視（flug=1を無視）
                 pc.printf("%d %c  flag:%d gp:%d\n",a,gps_c[a],flag,gp);
-                }
+              }
                  
             else if(gps_c[a-1] == '\r' && gps_c[a] == '\n' && flag == gp)
-            {
+              {
                 gps_c[a+1]='\0';
                 flag = 10;
-            }
-            else if(a > 256)//aが256を超えたとき、ループを強制脱出
-            {   
+              }
+              
+              
+            else if(a > 256)//aが256を超えたとき、ループを強制脱出(whileから)
+              {   
                 count_gps = 5;
                 flag = 10;
-            }
+              }
 
         }
 
@@ -261,11 +264,24 @@
     if(y_min>y_rd && y_rd != 0) {
         y_min=y_rd;
     }
-
+    
+    if(count_offset > 300)
+    {
     offset_x = (x_min+x_max)/2;
     offset_y = (y_min+y_max)/2;
     
     if(0 < offset_x)
+     { x_dat = con_x + offset_x; }
+    else if(0 > offset_x)
+     { x_dat = con_x - offset_x; }
+           
+    if(0 < offset_y)
+     { y_dat = con_y - offset_y; }
+    else if(0 > offset_y)
+     { y_dat = con_y + offset_y; }
+    
+    /*
+    if(0 < offset_x)
      { x_dat = con_x + offset_x*1.3; }
     else if(0 > offset_x)
      { x_dat = con_x - offset_x*1.3; }
@@ -273,7 +289,8 @@
     if(0 < offset_y)
      { y_dat = con_y + offset_y*1.3; }
     else if(0 > offset_y)
-     { y_dat = con_y - offset_y*1.3; }
+     { y_dat = con_y - offset_y*1.3; }*/
+    }
     
     pc.printf("x: %d , y: %d , x_dat: %f , y_dat: %f , x_max: %.5f , x_min: %.5f , direction: %f \n",con_x, con_y, x_rd, y_rd, x_max, x_min, direction);
     return 0;
@@ -281,14 +298,14 @@
 
 void cal_gps()
 {
-    if(latitude_target > latitude1 && longitude_target > longitude1)//第１象限
+    /*if(latitude_target > latitude1 && longitude_target > longitude1)//第１象限
      {quadrant = 1;}
     if(latitude_target > latitude1 && longitude_target < longitude1)
      {quadrant = 2;}
     if(latitude_target < latitude1 && longitude_target < longitude1)
      {quadrant = 3;}
     if(latitude_target < latitude1 && longitude_target < longitude1)
-     {quadrant = 4;}
+     {quadrant = 4;}*/
      
     y_target = (latitude_target-latitude1)*111319.49;
     x_target = (longitude_target-longitude1)*cos(latitude1*(3.1415926535/180))*111319.49;
@@ -387,12 +404,9 @@
           mag.getY(&con_y);
           offset(con_x,con_y);//x,y オフセット計算
           cal_con();
-                   
-          gps_read();
-          gps_save();
-          cal_gps();
-          motor_l.pulsewidth(motor_palse/2);
-          motor_r.pulsewidth(motor_palse/4);
+          
+          motor_l.pulsewidth(motor_palse*0.7);
+          motor_r.pulsewidth(motor_palse*0.35);
           }}
                    
    else if(direction < direction_target-2.0)
@@ -408,19 +422,13 @@
           mag.getY(&con_y);
           offset(con_x,con_y);//x,y オフセット計算
           cal_con();
-                   
-          gps_read();
-          gps_save();
-          cal_gps();
-          motor_r.pulsewidth(motor_palse/2);
-          motor_l.pulsewidth(motor_palse/4);}}
-                   
-  else if(direction <= direction_target+2 && direction >= direction_target-2)
-    {   
-        pc.printf("front \n");
-        motor_l.pulsewidth(motor_palse*0.8);  //パルス幅
+          
+          motor_r.pulsewidth(motor_palse*0.7);
+          motor_l.pulsewidth(motor_palse*0.35);
+          }}
+    
         motor_r.pulsewidth(motor_palse*0.8);
-    }
+        motor_l.pulsewidth(motor_palse*0.8);
                  
         pc.printf("direction: %f  direction_target: %f\n",direction,direction_target);
 
@@ -500,7 +508,7 @@
                    motor_l.pulsewidth(motor_palse);
                    }
                    
-                if(direction <= direction_target+2 && direction >= direction_target-2)
+                if(direction <= direction_target+2.0 && direction >= direction_target-2.0)
                  { pc.printf("front \n");
                    motor_l.pulsewidth(motor_palse);  //パルス幅
                    motor_r.pulsewidth(motor_palse);
@@ -512,24 +520,26 @@
                 
                 break;
         case 3:
-                motor_l.pulsewidth(motor_palse*0.4);  //パルス幅
-                motor_r.pulsewidth(motor_palse*0.4); 
+                motor_l.pulsewidth(0.0);  //パルス幅
+                motor_r.pulsewidth(0.0);
                 
-                if(direction >= direction_target+2 && direction <= direction_target-2)
-                 { pc.printf("trun \n");
-                   turn_sequence();}        
-        
                 gps_read();
                 gps_save();
                 cal_gps();
+                
+                mag.getX(&con_x);//コンパス読み込み
+                mag.getY(&con_y);
+                offset(con_x,con_y);//x,y オフセット計算
+                cal_con();
+                
+                turn_sequence();
+                
+                motor_l.pulsewidth(motor_palse*0.8);  //パルス幅
+                motor_r.pulsewidth(motor_palse*0.8);
+                
         
                 longitudegosa = longitude_target-longitude1;
                 latitudegosa = latitude_target-latitude1;
-        
-                motor_l.pulsewidth(motor_palse*0.8);  //パルス幅
-                motor_r.pulsewidth(motor_palse*0.8);
-                
-                turn_sequence();
                 
                 if( abs(longitudegosa)<=0.1 && abs(latitudegosa)<=0.1 )
                  { pc.printf("front \n");
@@ -551,6 +561,7 @@
                    count_move++;}
                 else
                  {
+                  turn_sequence();
                   motor_l.pulsewidth(motor_palse/2);  //パルス幅
                   motor_r.pulsewidth(motor_palse/2);
                  }
@@ -603,6 +614,10 @@
     mag.getX(&con_x);//コンパス読み込み
     mag.getY(&con_y);
     mag.getZ(&con_z);
+    
+    motor_r.pulsewidth(motor_palse*0.7);
+    motor_l.pulsewidth(motor_palse*0.35);
+    
     for(count_offset=0; count_offset < 300; count_offset++)//300個分のサンプ取得
      {
       mag.getX(&con_x);//コンパス読み込み
@@ -618,6 +633,8 @@
                       fclose(fp);
         }
       }
+    motor_r.pulsewidth(motor_palse*0.0);
+    motor_l.pulsewidth(motor_palse*0.0);
     bit1 = 0;
     
     while(1) {