Diff: main.cpp

Revision:

2:6f9881435f17

Parent:

1:2992de38cf3e

Child:

3:8d904225adfd

--- a/main.cpp	Thu Feb 23 00:24:24 2017 +0000
+++ b/main.cpp	Sun Feb 26 16:46:37 2017 +0000
@@ -32,7 +32,8 @@
 DigitalOut bit2(LED3);
 DigitalOut bit3(LED2);
 DigitalOut bit4(LED1);
-DigitalOut bit5(p16);
+//DigitalOut bit5(p16);
+DigitalIn local_on2(p16);
 DigitalOut stby(p11);
 DigitalOut motor_io(p12);
 PwmOut servo(p24);
@@ -46,15 +47,15 @@
 char gps_c[256];
 float palse=0.02;
 float servo_palse=0.002;
-float motor_palse=0.01;
+float motor_palse=0.02;
 float sig_time;
-int offset_x,offset_y;
+float offset_x,offset_y;
 float cds_on,x_max,y_max,x_min,y_min;
 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.11707;
-float latitude_target = 139.4720;
+float longitude_target = 36.065726;//36.11707
+float latitude_target = 139.272254;//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;
@@ -261,11 +262,18 @@
         y_min=y_rd;
     }
 
-    offset_x = ((abs(x_min)+abs(x_max))/2);
-    offset_y = ((abs(y_min)+abs(y_max))/2);
+    offset_x = (x_min+x_max)/2;
+    offset_y = (y_min+y_max)/2;
     
-    x_dat = x_rd + offset_x;
-    y_dat = y_rd - 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; }
+           
+    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; }
     
     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;
@@ -287,21 +295,21 @@
     d_target = sqrt(pow(con_x,2.0)*pow(con_y,2.0));//目的地までの距離
     
     angle_caly = cos(latitude_target)*sin(longitude_target-longitude1);
-    angle_calx = cos(latitude1)*sin(latitude_target)-sin(latitude1)*cos(latitude_target)*cos(longitude_target-longitude1);
+    angle_calx = cos(latitude1)*(sin(latitude_target)-sin(latitude1))*cos(latitude_target)*cos(longitude_target-longitude1);
     sita = atan2(angle_caly,angle_calx);
     if(sita < 0)
      {sita = sita + 2*3.1415926535;}
-    direction_target = sita*180/3.1415926535;
+    direction_target = sita*57.29578;
 }
 
 void cal_con()
 {
 
-    
+    direction = atan2(y_dat,x_dat);
     if(direction < 0)
      {direction = direction + 2*3.1415926535;}
-    direction = (atan(y_dat/x_dat))*57.2958;//ラジアンに変換
-    //direction = (atan(y_cal/x_cal))*57.2958;
+    direction = direction*57.29578; //ラジアンに変換
+    //direction = (atan(y_cal/x_cal))*57.29578;
 
     if(0 < direction && 89 > direction){
         angle_c = 'N';}
@@ -317,26 +325,6 @@
 
     else{ angle_c = '?';}
 
-    /*if(north == 1) {
-        angle_c = 'N';
-    }
-
-    else if(east == 1) {
-        angle_c = 'E';
-    }
-
-    else if(south == 1) {
-        angle_c = 'S';
-    }
-
-    else if(west == 1) {
-        angle_c = 'W';
-    }
-
-    else {
-        angle_c = '?';
-    }*/
-
 }
 
 void trigger()
@@ -378,15 +366,77 @@
      {count_move=1;}
 }
 
+void turn_sequence()
+{   
+    __disable_irq();
+    pc.printf("************turn_sequence*************\n");
+    pc.printf("direction: %f  direction_target: %f\n",direction,direction_target);
+    longitudegosa = longitude_target-longitude1;
+    latitudegosa = latitude_target-latitude1;
+                
+    if(direction > direction_target+2.0)
+      { while(direction > direction_target+2.0)
+        {
+          pc.printf("turn clockwise \ndirection: %f  direction_target: %f\n",direction,direction_target);
+                   
+          FILE *fp = fopen("/local/turn.txt","a");
+          fprintf(fp,"%c,%d,%d,%f,%f,%f,%f\n",angle_c,con_x,con_y,x_dat,y_dat,direction_target,direction);
+          fclose(fp);
+                   
+          mag.getX(&con_x);//コンパス読み込み
+          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);
+          }}
+                   
+   else if(direction < direction_target-2.0)
+     { while(direction < direction_target-2.0)
+       {
+          pc.printf("turn anti clockwise \ndirection: %f  direction_target: %f\n",direction,direction_target);
+                   
+          FILE *fp = fopen("/local/turn2.txt","a");
+          fprintf(fp,"%c,%d,%d,%f,%f,%f,%f\n",angle_c,con_x,con_y,x_dat,y_dat,direction_target,direction);
+          fclose(fp);
+                   
+          mag.getX(&con_x);//コンパス読み込み
+          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.8);
+    }
+                 
+        pc.printf("direction: %f  direction_target: %f\n",direction,direction_target);
+
+        pc.printf("************turn_sequence end*************\n");
+    
+}
+
 void move()
 {
     switch(count_move)
     {
         case 1: 
-                bit5 = 1;
+                
                 pc.printf("*************case1**************\n");
-                motor_l.pulsewidth(0.001);  //パルス幅
-                motor_r.pulsewidth(0.001);
+                motor_l.pulsewidth(0.01);  //パルス幅
+                motor_r.pulsewidth(0.01);
                 wait(10);//落下時間
                 
                 servo.pulsewidth(servo_palse);
@@ -396,58 +446,109 @@
                 count_move++;
                 pc.printf("*************case1 end**************\n"); 
                 break;
+                
         case 2:
+                __disable_irq();
                 pc.printf("*************case2**************\n");
                 pc.printf("direction: %f  direction_target: %f\n",direction,direction_target);
                 longitudegosa = longitude_target-longitude1;
                 latitudegosa = latitude_target-latitude1;
-            
-                if(direction > direction_target+5.0)
-                 { while(direction > direction_target+5.0)
+                
+                if(direction > direction_target+2.0)
+                 { while(direction > direction_target+2.0)
                     {
-                   pc.printf("tarn clockwise \ndirection: %f  direction_target: %f\n",direction,direction_target);
+                   pc.printf("turn clockwise \ndirection: %f  direction_target: %f\n",direction,direction_target);
+                   
+                   FILE *fp = fopen("/local/turn.txt","a");
+                   fprintf(fp,"%c,%d,%d,%f,%f,%f,%f\n",angle_c,con_x,con_y,x_dat,y_dat,direction_target,direction);
+                   fclose(fp);
+                   
                    mag.getX(&con_x);//コンパス読み込み
                    mag.getY(&con_y);
                    offset(con_x,con_y);//x,y オフセット計算
                    cal_con();
-                   motor_l.pulsewidth(motor_palse/2);
-                   motor_r.pulsewidth(motor_palse/4);}}
+                   
+                   /*gps_read();
+                   gps_save();
+                   cal_gps();回転しすぎる*/
+                   motor_l.pulsewidth(motor_palse*0.7);
+                   motor_r.pulsewidth(motor_palse*0.35);}
+                   motor_r.pulsewidth(motor_palse);
+                   motor_l.pulsewidth(motor_palse);
+                   }
                    
-                else if(direction < direction_target-5.0)
-                 { while(direction < direction_target-5.0)
+                else if(direction < direction_target-2.0)
+                 { while(direction < direction_target-2.0)
                     {
-                   pc.printf("tarn anti clockwise \ndirection: %f  direction_target: %f\n",direction,direction_target);
+                   pc.printf("turn anti clockwise \ndirection: %f  direction_target: %f\n",direction,direction_target);
+                   
+                   FILE *fp = fopen("/local/turn2.txt","a");
+                   fprintf(fp,"%c,%d,%d,%f,%f,%f,%f\n",angle_c,con_x,con_y,x_dat,y_dat,direction_target,direction);
+                   fclose(fp);
+                   
                    mag.getX(&con_x);//コンパス読み込み
                    mag.getY(&con_y);
                    offset(con_x,con_y);//x,y オフセット計算
                    cal_con();
-                   motor_r.pulsewidth(motor_palse/2);
-                   motor_l.pulsewidth(motor_palse/4);}}
                    
-                else if(direction <= direction_target+5 && direction >= direction_target-5)
-                 {count_move++;}
+                   /*gps_read();
+                   gps_save();
+                   cal_gps();回転しすぎる*/
+                   motor_r.pulsewidth(motor_palse*0.7);
+                   motor_l.pulsewidth(motor_palse*0.35);}
+                   motor_r.pulsewidth(motor_palse);
+                   motor_l.pulsewidth(motor_palse);
+                   }
+                   
+                if(direction <= direction_target+2 && direction >= direction_target-2)
+                 { pc.printf("front \n");
+                   motor_l.pulsewidth(motor_palse);  //パルス幅
+                   motor_r.pulsewidth(motor_palse);
+                   count_move++;}
                  
-                if( abs(longitudegosa)<=50 && abs(latitudegosa)<=50 )
-                { pc.printf("front \n");
-                  motor_l.pulsewidth(motor_palse);  //パルス幅
-                  motor_r.pulsewidth(motor_palse);
-                  count_move++; }
                 pc.printf("direction: %f  direction_target: %f\n",direction,direction_target);
 
                 pc.printf("*************case2 end**************\n");
                 
                 break;
+        case 3:
+                motor_l.pulsewidth(motor_palse*0.4);  //パルス幅
+                motor_r.pulsewidth(motor_palse*0.4); 
                 
-        case 3:
+                if(direction >= direction_target+2 && direction <= direction_target-2)
+                 { pc.printf("trun \n");
+                   turn_sequence();}        
+        
+                gps_read();
+                gps_save();
+                cal_gps();
+        
+                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");
+                   count_move++; }
+                  
+                break;        
+        case 4:
                 pc.printf("*************case3**************\n");
+                
+                __enable_irq();
                 trigger();
                 //distance.rise(rise_echo);
                 //distance.fall(fall_echo);
                 
-                pc.printf("%f\n",sig_time);
+                pc.printf("sig_time: %f\n",sig_time);
                 
-                if(sig_time <= 150.0)//150mm
-                 (count_move++);
+                if(sig_time <= 100.0)//100mm以下に近づいたとき停止
+                 { pc.printf("sig_time is 100mm or under: %f\n",sig_time);
+                   count_move++;}
                 else
                  {
                   motor_l.pulsewidth(motor_palse/2);  //パルス幅
@@ -459,7 +560,7 @@
         default:
                 motor_l.pulsewidth(0.0);
                 motor_r.pulsewidth(0.0);
-                bit5 = 0;
+                
                 break;
         
     }
@@ -502,8 +603,21 @@
     mag.getX(&con_x);//コンパス読み込み
     mag.getY(&con_y);
     mag.getZ(&con_z);
-    for(count_offset=0; count_offset < 100; count_offset++)//300個分のサンプ取得
-     {offset(con_x,con_y);}
+    for(count_offset=0; count_offset < 300; count_offset++)//300個分のサンプ取得
+     {
+      mag.getX(&con_x);//コンパス読み込み
+      mag.getY(&con_y);
+      mag.getZ(&con_z);
+      offset(con_x,con_y);
+      cal_con();
+      
+      if(local_on2==1)
+        {
+           FILE *fp = fopen("/local/first_con.txt","a");
+                      fprintf(fp,"%c,%d,%d,%f,%f,%f,%f\n",angle_c,con_x,con_y,x_dat,y_dat,direction_target,direction);
+                      fclose(fp);
+        }
+      }
     bit1 = 0;
     
     while(1) {